odom aand nav

src/robot_navigation/CMakeLists.txt

@@ -0,0 +1,199 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(robot_navigation)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## 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
+  roscpp
+  rospy
+  std_msgs
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a run_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+#   DEPENDENCIES
+#   std_msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a run_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if you package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+#  INCLUDE_DIRS include
+#  LIBRARIES navigation
+#  CATKIN_DEPENDS roscpp rospy std_msgs
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+# include
+  ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+#   src/${PROJECT_NAME}/navigation.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+# add_executable(${PROJECT_NAME}_node src/navigation_node.cpp)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(${PROJECT_NAME}_node
+#   ${catkin_LIBRARIES}
+# )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_navigation.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)

src/robot_navigation/README.md

@@ -0,0 +1,53 @@
+# blackTornadoRobot建图导航功能
+建图导航功能包，可实现建图导航功能
+
+### 建图运行方法
+首先启动小车
+
+```sh
+$ roslaunch robot_bringup robot_bringup.launch
+```
+
+然后运行键盘控制程序
+
+```sh
+$ rosrun teleop_twist_keyboard teleop_twist_keyboard.py
+```
+
+再运行建图程序gmapping
+
+```sh
+$ roslaunch robot_navigation gmapping_ydlidar_X2L.launch
+```
+
+启动rviz可视化工具
+
+```sh
+$ roslaunch robot_navigation view_navigation.launch
+```
+用键盘控制小车移动可建立地图地图
+
+最后保存地图
+
+```sh
+$ rosrun map_server map_saver -f  ~/blackTornadoRobot/src/robot_navigation/maps/mymap
+```
+
+### 导航运行方法
+首先启动小车
+
+```sh
+$ roslaunch robot_bringup robot_bringup.launch
+```
+
+运行amcl导航
+
+```sh
+$ roslaunch robot_navigation amcl_ydlidar_X2L.launch
+```
+
+启动rviz可视化工具
+
+```sh
+$ roslaunch robot_navigation view_navigation.launch
+```

src/robot_navigation/config/base_global_planner_params.yaml

@@ -0,0 +1,24 @@
+# Copyright: 2016-2020 ROS小课堂 www.corvin.cn
+# Description: 三轮全向移动小车的全局路径规划参数配置文件.
+# Author: corvin
+# History:
+#   20200821: init this file.
+
+# Global Planner Selection
+base_global_planner: "global_planner/GlobalPlanner"
+
+GlobalPlanner:
+    allow_unknown: false
+    default_tolerance: 0.2
+    visualize_potential: false
+    use_dijkstra: true
+    use_quadratic: true
+    use_grid_path: false
+    old_navfn_behavior: false
+    lethal_cost: 253
+    neutral_cost: 50
+    cost_factor: 3
+    publish_potential: true
+    orientation_mode: 0
+    orientation_window_size: 1
+

src/robot_navigation/config/base_local_planner_params.yaml

@@ -0,0 +1,67 @@
+# Copyright(c): 2016-2020 ROS小课堂 www.corvin.cn
+# Author: corvin
+# Description:
+#  本地规划配置文件,各参数意义如下：
+#   max_vel_x:最大前进速度.
+#   min_vel_x:最小前进速度.
+#   max_vel_y:最大横向移动速度.
+#   min_vel_y:最小横向移动速度.
+#   max_vel_theta:最大旋转角速度.
+#   min_in_place_vel_theta:机器人最小原地旋转速度.
+#   escape_vel: 机器人后退速度,该值必须是负数. 
+# History:
+#   20180410: init this file.
+#   20180411: update param based on kinds.
+#
+
+recovery_behavior_enabled: true
+clearing_rotation_allowed: false
+
+TrajectoryPlannerROS:
+   #Robot Configuration Parameters
+   max_vel_x: 0.15
+   min_vel_x: 0.10
+   y_vels: [-0.17,-0.1,0.1,0.17]
+   max_vel_theta: 0.5
+   min_vel_theta: -0.5
+   min_in_place_vel_theta: 0.3
+   escape_vel: -0.10
+
+   acc_lim_x: 0.05
+   acc_lim_y: 0.05   # accelerator speed 
+   acc_lim_theta: 0.1
+
+   holonomic_robot: true
+
+   #Goal Tolerance Parameters
+   xy_goal_tolerance: 0.30  # 30cm
+   yaw_goal_tolerance: 0.3  # about 17 degrees
+   latch_xy_goal_tolerance: true
+
+   #Forward Simulation Parameters
+   sim_time: 2.0
+   sim_granularity: 0.025
+   angular_sim_granularity: 0.025
+   vx_samples: 5
+   vy_samples: 3 # omniwheel robot
+   vtheta_samples: 12 
+   
+   #Trajectory Scoring Parameters
+   meter_scoring: true
+   pdist_scale: 0.8
+   gdist_scale: 0.5
+   occdist_scale: 0.1
+
+   heading_lookahead: 0.325
+   heading_scoring: false
+   heading_scoring_timestep: 0.8
+   dwa: true
+   publish_cost_grid_pc: false
+
+   #Oscillation Prevention Parameters
+   oscillation_timeout: 3.0
+   oscillation_reset_dist: 0.5
+
+   #Global Plan Parameters
+   prune_plan: true
+

src/robot_navigation/config/costmap_common_params.yaml

@@ -0,0 +1,47 @@
+# Copyright(c):2016-2020 ROS小课堂 www.corvin.cn
+# Description:
+# 代价地图通用配置文件，各参数意义如下：
+#   obstacle_range: 最大障碍物检测范围,超过该范围不认为是障碍物
+#   raytrace_range: 检测自由空间的最大范围
+#   robot_radius: 机器人本体的半径大小,单位:米,圆形机器人使用.
+#   footprint: 对于非圆形机器人可以设置该参数,机器人中心[0,0]
+#     参数可以顺时针填写,也可以逆时针填写.
+#   inflation_radius: 与障碍物的安全半径距离,如果机器人经常撞到
+#     障碍物就需要增大该值,若无法通过狭窄地方就减小该值
+#   cost_scaling_factor:用于将inflation计算cost的比例因数,该参数
+#     越大,越不把这个inflating看着移动的cost.
+#   map_type:如果想要3D视图的就用voxel,2D视图的就用costmap参数.
+#   observation_sources: 观察源是激光雷达
+# Author: corvin
+# History:
+#   20180410:init this file.
+#   20200413:更新参数并更新注释信息.
+#
+
+
+#单独圆形机器人的参数
+#robot_radius: 0.13
+
+#圆形机器人前面带上下移动夹爪的参数
+footprint: [[ 0.22,  0.00],[ 0.22,  0.055],[ 0.10,  0.055],
+            [ 0.09,  0.09],[ 0.00,  0.13],[-0.09,  0.09],
+            [-0.13,  0.00],[-0.09, -0.09],[ 0.00, -0.13],
+            [ 0.09, -0.09],[ 0.10, -0.055],[ 0.22, -0.055]]
+map_type: costmap
+
+obstacle_layer:
+ enabled: true
+ obstacle_range: 1.0
+ raytrace_range: 1.5
+ inflation_radius: 0.05
+ observation_sources: scan
+ scan: {sensor_frame: lidar, data_type: LaserScan, topic: scan, marking: true, clearing: true}
+
+
+inflation_layer:
+ enabled: true
+ cost_scaling_factor: 5
+ inflation_radius:     0.15
+
+
+

src/robot_navigation/config/dwa_local_planner_params.yaml

@@ -0,0 +1,53 @@
+# Copyright: 2016-2020 www.corvin.cn ROS小课堂
+# Description:DWA局部规划器配置参数
+# Author: corvin
+# History:
+#   20200413:init this file.
+#
+
+base_local_planner: "dwa_local_planner/DWAPlannerROS"
+
+DWAPlannerROS:
+#robot configuration params
+  acc_lim_x: 0.3
+  acc_lim_y: 0
+  acc_lim_th: 0.3
+
+  max_vel_trans: 0.3
+  min_vel_trans: 0.25
+
+  max_vel_x: 0.35
+  min_vel_x: -0.25
+
+  max_vel_y: 0
+  min_vel_y: 0
+
+  max_vel_theta: 1
+  min_vel_theta: 0.5
+
+# Goal Tolerance Parametes
+  xy_goal_tolerance: 0.10
+  yaw_goal_tolerance: 0.17
+  latch_xy_goal_tolerance: false
+
+# Forward Simulation Parametesrs
+  sim_time: 1.5
+  vx_samples: 10
+  vy_samples: 0
+  vth_samples: 20
+  controller_frequency: 3
+
+# Trajectory Scoring Parameters
+  path_distance_bias: 32.0
+  goal_distance_bias: 20.0
+  occdist_scale: 0.02
+  forward_point_distance: 0.325
+  stop_time_buffer: 0.2
+  scaling_speed: 0.3
+  max_scaling_factor: 0.2
+
+# Oscillation Prevention Parameters
+  oscillation_reset_dist: 0.05
+
+# Global plan parameters
+  prune_plan: true

src/robot_navigation/config/global_costmap_params.yaml

@@ -0,0 +1,29 @@
+# Copyright(C): 2016-2020 www.corvin.cn ROS小课堂
+# Description:
+#   全局代价地图配置文件，各参数意义如下：
+#   global_frame: 全局代价地图参考系
+#   robot_base_frame: 指定机器人的基础参考系
+#   update_frequency: 指定地图更新频率，数值太大造成CPU负担重
+#   publish_frequency: 对于静态全局地图来说，不需不断发布
+#   static_map: 全局地图通常是静态的，因此设置为true
+#   rolling_window: 全局地图不会在机器人移动时候更新
+#   transform_tolerance: 指定在tf树中frame直接的转换最大延时，单位秒
+# Author: corvin
+# History:
+#   20180410: init this file.
+#   20200927:增加plugins参数,配置静态层和膨胀层.
+global_costmap:
+   global_frame: map
+   robot_base_frame: base_footprint
+
+   update_frequency: 0.1
+   publish_frequency: 1.0
+   static_map: true
+   rolling_window: false
+   transform_tolerance: 2.0
+
+   plugins:
+   - {name: static_layer,     type: "costmap_2d::StaticLayer"}
+   - {name: obstacle_layer,     type: "costmap_2d::VoxelLayer"}
+   - {name: inflation_layer,  type: "costmap_2d::InflationLayer"}
+

src/robot_navigation/config/local_costmap_params.yaml

@@ -0,0 +1,35 @@
+#Copyright(c):2016-2020 www.corvin.cn ROS小课堂
+#Description:
+#  本地代价地图配置文件，各参数意义如下：
+#  global_frame:指定本地代价地图参考系
+#  robot_base_frame:指定机器人的base_frame
+#  update_frequency:指定local代价地图更新频率
+#  publish_frequency: 代价地图发布可视化信息的频率
+#  static_map: 本地代价地图不会更新地图，设置false
+#  rolling_window: 设置滚动窗口,使机器人始终在窗体中心位置
+#  width: 代价地图宽度,滑动地图x维长度
+#  height:代价地图长度,滑动地图y维长度
+#  resolution: 代价地图的分辨率,该参数需要与yaml文件设置的地图
+#    分辨率匹配
+#Author: corvin
+#History:
+#  20180410: init this file.
+#  20200413:更新本地代价地图配置参数.
+#  20200927:添加plugins参数,配置障碍物层和膨胀层.
+local_costmap:
+    global_frame: odom_combined
+    robot_base_frame: base_footprint
+    update_frequency: 5.0
+    publish_frequency: 3.0
+
+    static_map: false
+    rolling_window: true
+    width: 2.0
+    height: 2.0
+    resolution: 0.02   #should same as map.yaml file
+    transform_tolerance: 2.0
+
+    plugins:
+    - {name: obstacle_layer,   type: "costmap_2d::VoxelLayer"}
+    - {name: inflation_layer,  type: "costmap_2d::InflationLayer"}
+

src/robot_navigation/config/teb_local_planner_params.yaml

@@ -0,0 +1,129 @@
+TebLocalPlannerROS:
+
+ odom_topic: odom
+    
+ # Trajectory
+  
+ teb_autosize: True
+ dt_ref: 0.3
+ dt_hysteresis: 0.1
+ max_samples: 500
+ global_plan_overwrite_orientation: False
+ allow_init_with_backwards_motion: False
+ max_global_plan_lookahead_dist: 3.0
+ global_plan_viapoint_sep: 0.05
+ global_plan_prune_distance: 1
+ exact_arc_length: False
+ feasibility_check_no_poses: 5
+ publish_feedback: False
+    
+ # Robot
+         
+ max_vel_x: 1.53 #1.5
+ max_vel_x_backwards: 0.2
+ max_vel_y: 1.0
+ max_vel_theta: 3.30 #3.0 
+ acc_lim_x: 0.35 #0.55
+ acc_lim_y: 0.5
+ acc_lim_theta: 1.1 #1.2
+ min_turning_radius: 0.0 # diff-drive robot (can turn on place!)
+
+ footprint_model:
+   type: "point"
+   #vertices: [[0.16, 0.12], [0.16, -0.12], [-0.16, -0.12], [-0.16, 0.12]]
+
+ # GoalTolerance
+    
+ xy_goal_tolerance: 0.78
+ yaw_goal_tolerance: 0.8
+ ifree_goal_vel: False
+ complete_global_plan: True
+    
+
+ # Trajectory
+
+ teb_autosize: True
+ dt_ref: 0.3
+ dt_hysteresis: 0.1
+ max_samples: 500
+ global_plan_overwrite_orientation: True
+ allow_init_with_backwards_motion: False
+ max_global_plan_lookahead_dist: 3.0
+ global_plan_viapoint_sep: 0.05
+ global_plan_prune_distance: 1
+ exact_arc_length: False
+ feasibility_check_no_poses: 2
+ publish_feedback: False
+
+ # Obstacles
+    
+ min_obstacle_dist: 0.3 # This value must also include our robot radius, since footprint_model is set to "point".
+ inflation_dist: 0.6
+ include_costmap_obstacles: True
+ costmap_obstacles_behind_robot_dist: 1.0
+ obstacle_poses_affected: 500.0
+
+ dynamic_obstacle_inflation_dist: 0.6
+ include_dynamic_obstacles: True
+
+ costmap_converter_plugin: ""
+ costmap_converter_spin_thread: True
+ costmap_converter_rate: 5
+
+ # Optimization
+    
+ no_inner_iterations: 5
+ no_outer_iterations: 4
+ optimization_activate: True
+ optimization_verbose: False
+ penalty_epsilon: 0.1
+ obstacle_cost_exponent: 4
+ weight_max_vel_x: 2
+ weight_max_vel_theta: 1
+ weight_acc_lim_x: 1
+ weight_acc_lim_theta: 1
+ weight_kinematics_nh: 10
+ weight_kinematics_forward_drive: 1
+ weight_kinematics_turning_radius: 1
+ weight_optimaltime: 4 # must be > 0 
+ weight_shortest_path: 0
+ weight_obstacle: 50
+ weight_inflation: 0.2
+ weight_dynamic_obstacle: 10
+ weight_dynamic_obstacle_inflation: 0.2
+ weight_viapoint: 1
+ weight_adapt_factor: 2
+
+ # Homotopy Class Planner
+
+ enable_homotopy_class_planning: True
+ enable_multithreading: True
+ max_number_classes: 4
+ selection_cost_hysteresis: 1.0
+ selection_prefer_initial_plan: 0.9
+ selection_obst_cost_scale: 1.0
+ selection_alternative_time_cost: False
+ 
+ roadmap_graph_no_samples: 15
+ roadmap_graph_area_width: 5
+ roadmap_graph_area_length_scale: 1.0
+ h_signature_prescaler: 0.5
+ h_signature_threshold: 0.1
+ obstacle_heading_threshold: 0.45
+ switching_blocking_period: 0.0
+ viapoints_all_candidates: True
+ delete_detours_backwards: True
+ max_ratio_detours_duration_best_duration: 3.0
+ visualize_hc_graph: False
+ visualize_with_time_as_z_axis_scale: False
+
+# Recovery
+ 
+ shrink_horizon_backup: True
+ shrink_horizon_min_duration: 10
+ oscillation_recovery: True
+ oscillation_v_eps: 0.1
+ oscillation_omega_eps: 0.1
+ oscillation_recovery_min_duration: 10
+ oscillation_filter_duration: 10
+

src/robot_navigation/launch/amcl_ls01d_lidar.launch

@@ -0,0 +1,69 @@
+<!--
+  Copyright(C): 2016-2018 ROS小课堂
+  Author: www.corvin.cn
+  Description:
+   ls01d lidar to amcl, first roslaunch carebot_bringup.launch, then load map,then startup 
+   ls01d lidar to scan,then roslaunch move_base package, last roslaunch amcl package.
+  History:
+    20180408: init this file.
+    20180411: add first roslaunch carebot_bringup.launch.
+-->
+
+<launch>
+  <!-- (1) first roslaunch carebot_bringup.launch -->
+  <include file="$(find carebot_bringup)/launch/carebot_bringup.launch" />
+
+  <!-- (2) Run the map server with a map -->
+  <node name="map_server" pkg="map_server" type="map_server" args="$(find carebot_navigation)/maps/mymap.yaml" />
+
+  <!-- (3) Startup leishen ls01d_lidar lidar node -->
+  <include file="$(find ls01d_lidar)/launch/ls01d_lidar.launch" />
+
+  <!-- (4) Startup move_base node -->
+  <include file="$(find carebot_navigation)/launch/move_base.launch" />
+
+  <!-- (5) Run amcl -->
+  <node pkg="amcl" type="amcl" name="carebot_amcl" >
+    <param name="odom_model_type" value="omni"/> #omni wheel mobilebase
+
+    <param name="base_frame_id" value="base_footprint"/>
+    <param name="global_frame_id" value="map" />
+    <param name="odom_frame_id" value="odom_combined" />
+
+    <param name="use_map_topic" value="true" />
+    <param name="odom_alpha5" value="0.1" />
+    <param name="transform_tolerance" value="0.5" />
+    <param name="gui_publish_rate" value="1.0" />
+    <param name="laser_max_beams" value="300" />
+    <param name="min_particles" value="500"/>
+    <param name="max_particles" value="5000"/>
+    <param name="kld_err" value="0.1"/>
+    <param name="kld_z" value="0.99"/>
+
+    <param name="odom_alpha1" value="0.2"/>
+    <param name="odom_alpha2" value="0.2"/>
+    <param name="odom_alpha3" value="0.2"/>
+    <param name="odom_alpha4" value="0.2"/>
+    <param name="odom_alpha5" value="0.2"/>
+
+    <param name="laser_z_hit" value="0.9"/>
+    <param name="laser_z_short" value="0.05"/>
+    <param name="laser_z_max" value="0.05"/>
+    <param name="laser_z_rand" value="0.05"/>
+    <param name="laser_sigma_hit" value="0.2"/>
+    <param name="laser_lambda_short" value="0.1"/>
+    <param name="laser_model_type" value="likelihood_field"/>
+    <param name="laser_model_type" value="beam"/>
+    <param name="laser_min_range" value="0.14"/>
+    <param name="laser_max_range" value="7.0"/>
+    <param name="laser_likelihood_max_dist" value="2.0"/>
+    <param name="update_min_d" value="0.2"/>
+    <param name="update_min_a" value="0.5"/>
+    <param name="resample_interval" value="2"/>
+    <param name="transform_tolerance" value="3.0"/>
+    <param name="recovery_alpha_slow" value="0.0"/>
+    <param name="recovery_alpha_fast" value="0.0"/>
+  </node>
+
+</launch>
+

src/robot_navigation/launch/amcl_rplidar_a1.launch

@@ -0,0 +1,53 @@
+<launch>
+  <!-- Run the map server with a map -->
+  <node name="map_server" pkg="map_server" type="map_server" args="$(find robot_navigation)/maps/my_room.yaml" />
+
+  <!-- startup rplidar node -->
+  <include file="$(find robot_navigation)/launch/rplidar.launch" />
+
+  <!-- startup move_base node -->
+  <include file="$(find robot_navigation)/launch/move_base.launch" />
+
+  <!-- Run amcl -->
+  <node pkg="amcl" type="amcl" name="robot_amcl">
+    <param name="odom_model_type" value="omni"/>
+
+    <param name="base_frame_id" value="base_footprint"/>
+    <param name="global_frame_id" value="map"/>
+    <param name="odom_frame_id" value="odom_combined"/>
+
+    <param name="use_map_topic" value="true"/>
+    <param name="odom_alpha5" value="0.1"/>
+    <param name="transform_tolerance" value="0.5" />
+    <param name="gui_publish_rate" value="1.0"/>
+    <param name="laser_max_beams" value="300"/>
+    <param name="min_particles" value="500"/>
+    <param name="max_particles" value="5000"/>
+    <param name="kld_err" value="0.1"/>
+    <param name="kld_z" value="0.99"/>
+    <param name="odom_alpha1" value="0.2"/>
+    <param name="odom_alpha2" value="0.2"/>
+    <param name="odom_alpha3" value="0.2"/>
+    <param name="odom_alpha4" value="0.2"/>
+    <param name="odom_alpha5" value="0.2"/>
+
+    <param name="laser_z_hit" value="0.9"/>
+    <param name="laser_z_short" value="0.05"/>
+    <param name="laser_z_max" value="0.05"/>
+    <param name="laser_z_rand" value="0.05"/>
+    <param name="laser_sigma_hit" value="0.2"/>
+    <param name="laser_lambda_short" value="0.1"/>
+    <param name="laser_model_type" value="likelihood_field"/>
+    <param name="laser_model_type" value="beam"/>
+    <param name="laser_min_range" value="0.14"/>
+    <param name="laser_max_range" value="7.0"/>
+    <param name="laser_likelihood_max_dist" value="2.0"/>
+    <param name="update_min_d" value="0.2"/>
+    <param name="update_min_a" value="0.5"/>
+    <param name="resample_interval" value="2"/>
+    <param name="transform_tolerance" value="3.0"/>
+    <param name="recovery_alpha_slow" value="0.0"/>
+    <param name="recovery_alpha_fast" value="0.0"/>
+  </node>
+</launch>
+

src/robot_navigation/launch/amcl_rplidar_a2.launch

@@ -0,0 +1,54 @@
+<launch>
+  <!-- Run the map server with a map -->
+  <node name="map_server" pkg="map_server" type="map_server" args="$(find carebot_navigation)/maps/mymap.yaml" />
+
+  <!-- startup rplidar node -->
+  <include file="$(find rplidar_ros)/launch/rplidar.launch" />
+
+  <!-- startup move_base node -->
+  <include file="$(find carebot_navigation)/launch/move_base.launch" />
+
+  <!-- Run amcl -->
+  <node pkg="amcl" type="amcl" name="carebot_amcl">
+    <param name="odom_model_type" value="omni"/>
+
+    <param name="base_frame_id" value="base_footprint"/>
+    <param name="global_frame_id" value="map"/>
+    <param name="odom_frame_id" value="odom"/>
+
+    <param name="use_map_topic" value="true"/>
+    <param name="odom_alpha5" value="0.1"/>
+    <param name="transform_tolerance" value="0.5" />
+    <param name="gui_publish_rate" value="1.0"/>
+    <param name="laser_max_beams" value="300"/>
+    <param name="min_particles" value="500"/>
+    <param name="max_particles" value="5000"/>
+    <param name="kld_err" value="0.1"/>
+    <param name="kld_z" value="0.99"/>
+    <param name="odom_alpha1" value="0.2"/>
+    <param name="odom_alpha2" value="0.2"/>
+    <param name="odom_alpha3" value="0.2"/>
+    <param name="odom_alpha4" value="0.2"/>
+    <param name="odom_alpha5" value="0.2"/>
+
+    <param name="laser_z_hit" value="0.9"/>
+    <param name="laser_z_short" value="0.05"/>
+    <param name="laser_z_max" value="0.05"/>
+    <param name="laser_z_rand" value="0.05"/>
+    <param name="laser_sigma_hit" value="0.2"/>
+    <param name="laser_lambda_short" value="0.1"/>
+    <param name="laser_model_type" value="likelihood_field"/>
+    <param name="laser_model_type" value="beam"/>
+    <param name="laser_min_range" value="0.14"/>
+    <param name="laser_max_range" value="7.0"/>
+    <param name="laser_likelihood_max_dist" value="2.0"/>
+    <param name="update_min_d" value="0.2"/>
+    <param name="update_min_a" value="0.5"/>
+    <param name="resample_interval" value="2"/>
+    <param name="transform_tolerance" value="3.0"/>
+    <param name="recovery_alpha_slow" value="0.0"/>
+    <param name="recovery_alpha_fast" value="0.0"/>
+  </node>
+
+</launch>
+

src/robot_navigation/launch/amcl_ydlidar_X2L.launch

@@ -0,0 +1,60 @@
+<!--
+  Copyright: 2016-2020 www.corvin.cn ROS小课堂
+  Description:使用EAI的X2L雷达进行amcl定位，加载地图进行路径规划，自动导航
+  Author: corvin
+  History:
+    20200326: init this file.
+    20200804:将odom_mode_type改为omni-corrected,对应odom_alpha参数也更新.
+-->
+<launch>
+  <!-- Run the map server with a map -->
+  <node name="map_server" pkg="map_server" type="map_server" args="$(find robot_navigation)/maps/mymap.yaml" />
+
+  <!-- startup ydlidar X2L node -->
+  <include file="$(find robot_navigation)/launch/ydlidar_X2L.launch" />
+
+  <!-- startup move_base node -->
+  <include file="$(find robot_navigation)/launch/move_base.launch" />
+
+  <!-- Run amcl node -->
+  <node pkg="amcl" type="amcl" name="robot_amcl">
+    <param name="odom_model_type" value="diff-corrected"/>
+
+    <param name="base_frame_id" value="base_footprint"/>
+    <param name="global_frame_id" value="map"/>
+    <param name="odom_frame_id" value="odom_combined"/>
+
+    <param name="use_map_topic" value="false"/>
+    <param name="transform_tolerance" value="0.1" />
+    <param name="gui_publish_rate" value="10.0"/>
+    <param name="laser_max_beams" value="300"/>
+    <param name="min_particles" value="500"/>
+    <param name="max_particles" value="5000"/>
+    <param name="kld_err" value="0.1"/>
+    <param name="kld_z" value="0.99"/>
+    <param name="odom_alpha1" value="0.2"/>
+    <param name="odom_alpha2" value="0.2"/>
+    <param name="odom_alpha3" value="0.2"/>
+    <param name="odom_alpha4" value="0.2"/>
+    <param name="odom_alpha5" value="0.2"/>
+
+    <param name="laser_z_hit" value="0.9"/>
+    <param name="laser_z_short" value="0.05"/>
+    <param name="laser_z_max" value="0.05"/>
+    <param name="laser_z_rand" value="0.05"/>
+    <param name="laser_sigma_hit" value="0.2"/>
+    <param name="laser_lambda_short" value="0.1"/>
+    <param name="laser_model_type" value="likelihood_field"/>
+    <param name="laser_model_type" value="beam"/>
+    <param name="laser_min_range" value="0.14"/>
+    <param name="laser_max_range" value="7.0"/>
+    <param name="laser_likelihood_max_dist" value="2.0"/>
+    <param name="update_min_d" value="0.05"/>
+    <param name="update_min_a" value="0.5"/>
+    <param name="resample_interval" value="2"/>
+    <param name="transform_tolerance" value="3.0"/>
+    <param name="recovery_alpha_slow" value="0.0"/>
+    <param name="recovery_alpha_fast" value="0.0"/>
+  </node>
+</launch>
+

src/robot_navigation/launch/gazebo.launch

@@ -0,0 +1,20 @@
+<launch>
+  <include
+    file="$(find gazebo_ros)/launch/empty_world.launch" />
+  <node
+    name="tf_footprint_base"
+    pkg="tf"
+    type="static_transform_publisher"
+    args="0 0 0 0 0 0 base_link base_footprint 40" />
+  <node
+    name="spawn_model"
+    pkg="gazebo_ros"
+    type="spawn_model"
+    args="-file $(find carebot_description)/urdf/carebot.urdf -urdf -model carebot"
+    output="screen" />
+  <node
+    name="fake_joint_calibration"
+    pkg="rostopic"
+    type="rostopic"
+    args="pub /calibrated std_msgs/Bool true" />
+</launch>

src/robot_navigation/launch/gmapping_ls01d_lidar.launch

@@ -0,0 +1,60 @@
+<!--
+  Copyright(C): 2016-2018 ROS小课堂
+  Author: www.corvin.cn
+  Description:
+    gmapping with ls01d lidar, first roslaunch carebot_bringup.launch.
+  History:
+    20180411:init this file.
+-->
+
+<launch>
+  <arg name="scan_topic"  default="scan" />
+  <arg name="base_frame"  default="base_footprint"/>
+  <arg name="odom_frame"  default="odom"/>
+
+  <!-- first roslaunch carebot_bringup -->
+  <include file="$(find carebot_bringup)/launch/carebot_bringup.launch" />
+
+  <!-- second startup ls01d lidar -->
+  <include file="$(find ls01d_lidar)/launch/ls01d_lidar.launch" />
+
+  <!-- third start gmapping node -->
+  <node pkg="gmapping" type="slam_gmapping" name="simple_gmapping" output="screen">
+      <param name="base_frame" value="$(arg base_frame)"/>
+      <param name="odom_frame" value="$(arg odom_frame)"/>
+      <param name="map_update_interval" value="5.0"/>
+      <param name="maxUrange" value="8.0"/>
+      <param name="sigma" value="0.05"/>
+      <param name="kernelSize" value="1"/>
+      <param name="lstep" value="0.05"/>
+      <param name="astep" value="0.05"/>
+      <param name="iterations" value="5"/>
+      <param name="lsigma" value="0.075"/>
+      <param name="ogain" value="3.0"/>
+      <param name="lskip" value="0"/>
+      <param name="minimumScore" value="50"/>
+
+      <param name="srr" value="0.1"/>
+      <param name="srt" value="0.2"/>
+      <param name="str" value="0.1"/>
+      <param name="stt" value="0.2"/>
+
+      <param name="linearUpdate" value="0.3"/>
+      <param name="angularUpdate" value="0.4"/>
+      <param name="temporalUpdate" value="3.0"/>
+      <param name="resampleThreshold" value="0.5"/>
+      <param name="particles" value="30"/>
+
+      <param name="xmin" value="-5.0" />
+      <param name="ymin" value="-5.0" />
+      <param name="xmax" value="5.0" />
+      <param name="ymax" value="5.0" />
+      <param name="delta" value="0.05" />
+
+      <param name="llsamplerange" value="0.01" />
+      <param name="llsamplestep" value="0.01" />
+      <param name="lasamplerange" value="0.005" />
+      <param name="lasamplestep" value="0.005" />
+  </node>
+</launch>
+

src/robot_navigation/launch/gmapping_rplidar_a1.launch

@@ -0,0 +1,43 @@
+<launch>
+  <param name="use_sim_time" value="false"/>  
+
+  <!-- startup rplidar node -->
+  <include file="$(find robot_navigation)/launch/rplidar.launch" />
+
+  <!-- startup move_base node -->
+  <include file="$(find robot_navigation)/launch/move_base.launch" />
+
+  <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen">  
+    <param name="odom_frame" value="odom_combined" />
+
+    <param name="map_update_interval" value="5.0"/>
+    <param name="maxUrange" value="7.5"/>
+    <param name="sigma" value="0.05"/>
+    <param name="kernelSize" value="1"/>
+    <param name="lstep" value="0.05"/>
+    <param name="astep" value="0.05"/>
+    <param name="iterations" value="5"/>
+    <param name="lsigma" value="0.075"/>
+    <param name="ogain" value="3.0"/>
+    <param name="lskip" value="0"/>
+    <param name="minimumScore" value="50"/>
+    <param name="srr" value="0.1"/>
+    <param name="srt" value="0.2"/>
+    <param name="str" value="0.1"/>
+    <param name="stt" value="0.2"/>
+    <param name="linearUpdate" value="1.0"/>
+    <param name="angularUpdate" value="0.5"/>
+    <param name="temporalUpdate" value="3.0"/>
+    <param name="resampleThreshold" value="0.5"/>
+    <param name="particles" value="30"/>
+    <param name="xmin" value="-5.0"/>
+    <param name="ymin" value="-5.0"/>
+    <param name="xmax" value="5.0"/>
+    <param name="ymax" value="5.0"/>
+    <param name="delta" value="0.05"/>
+    <param name="llsamplerange" value="0.01"/>
+    <param name="llsamplestep" value="0.01"/>
+    <param name="lasamplerange" value="0.005"/>
+    <param name="lasamplestep" value="0.005"/>
+  </node>
+</launch>

src/robot_navigation/launch/gmapping_rplidar_a2.launch

@@ -0,0 +1,43 @@
+<launch>  
+  <param name="use_sim_time" value="false"/>  
+
+  <!-- startup rplidar node -->
+  <include file="$(find rplidar_ros)/launch/rplidar.launch" />
+
+  <!-- startup move_base node -->
+  <include file="$(find carebot_navigation)/launch/move_base.launch" />
+
+  <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen">  
+    <param name="map_update_interval" value="5.0"/>  
+    <param name="maxUrange" value="7.5"/>  
+    <param name="sigma" value="0.05"/>  
+    <param name="kernelSize" value="1"/>  
+    <param name="lstep" value="0.05"/>  
+    <param name="astep" value="0.05"/>  
+    <param name="iterations" value="5"/>  
+    <param name="lsigma" value="0.075"/>  
+    <param name="ogain" value="3.0"/>  
+    <param name="lskip" value="0"/>  
+    <param name="minimumScore" value="50"/>  
+    <param name="srr" value="0.1"/>  
+    <param name="srt" value="0.2"/>  
+    <param name="str" value="0.1"/>  
+    <param name="stt" value="0.2"/>  
+    <param name="linearUpdate" value="1.0"/>  
+    <param name="angularUpdate" value="0.5"/>  
+    <param name="temporalUpdate" value="3.0"/>  
+    <param name="resampleThreshold" value="0.5"/>  
+    <param name="particles" value="30"/>  
+    <param name="xmin" value="-5.0"/>  
+    <param name="ymin" value="-5.0"/>  
+    <param name="xmax" value="5.0"/>  
+    <param name="ymax" value="5.0"/>  
+    <param name="delta" value="0.05"/>  
+    <param name="llsamplerange" value="0.01"/>  
+    <param name="llsamplestep" value="0.01"/>  
+    <param name="lasamplerange" value="0.005"/>  
+    <param name="lasamplestep" value="0.005"/>  
+  </node>  
+
+</launch>  
+

src/robot_navigation/launch/gmapping_ydlidar_X2L.launch

@@ -0,0 +1,59 @@
+<!--
+  Copyright: 2016-2020 www.corvin.cn ROS小课堂
+  Description:使用EAI的X2L雷达进行gamapping建图,整个过程如下:
+    首先启动x2l雷达,然后启动move_base节点,最后启动gmapping节点.
+    所有节点启动成功后,即可开始gmapping建图,在rviz中可以查看整个
+    建图过程.下面来解释一下gmapping操作的重点参数:
+    1):delta:调节生成地图的分辨率,单位米.
+  Author: corvin
+  History:
+    20200325:init this file.
+-->
+<launch>
+  <param name="use_sim_time" value="false"/>
+
+  <!-- startup EAI X2L lidar node -->
+  <include file="$(find robot_navigation)/launch/ydlidar_X2L.launch" />
+
+  <!-- startup move_base node --><!-- 建图不需要启动move_base -->
+  <!-- <include file="$(find robot_navigation)/launch/move_base.launch" /> -->
+
+  <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen">  
+    <param name="odom_frame" value="odom_combined" />
+
+    <param name="map_update_interval" value="5.0"/>
+    <param name="maxUrange" value="7.5"/>
+    <param name="sigma" value="0.05"/>
+    <param name="kernelSize" value="1"/>
+    <param name="lstep" value="0.05"/>
+    <param name="astep" value="0.05"/>
+    <param name="iterations" value="5"/>
+    <param name="lsigma" value="0.075"/>
+    <param name="ogain"  value="3.0"/>
+    <param name="lskip"  value="0"/>
+    <param name="minimumScore" value="50"/>
+
+    <param name="srr" value="0.1"/>
+    <param name="srt" value="0.2"/>
+    <param name="str" value="0.1"/>
+    <param name="stt" value="0.2"/>
+
+    <param name="linearUpdate"   value="1.0"/>
+    <param name="angularUpdate"  value="0.5"/>
+    <param name="temporalUpdate" value="3.0"/>
+    <param name="resampleThreshold" value="0.5"/>
+    <param name="particles" value="30"/>
+
+    <param name="xmin" value="-5.0"/>
+    <param name="ymin" value="-5.0"/>
+    <param name="xmax" value="5.0"/>
+    <param name="ymax" value="5.0"/>
+    <param name="delta" value="0.02"/> <!--resolution of the map --><!--大环境建图范围为0.05-0.1 -->
+
+    <param name="llsamplerange" value="0.01"/>
+    <param name="llsamplestep"  value="0.01"/>
+    <param name="lasamplerange" value="0.005"/>
+    <param name="lasamplestep"  value="0.005"/>
+  </node>
+</launch>
+

src/robot_navigation/launch/hector_mapping.launch

@@ -0,0 +1,60 @@
+<!--
+  Copyright: 2016-2018 www.corvin.cn
+  Author: corvin
+  Description: hector mapping.
+  History:
+    20180601: initial this file.
+-->
+<launch>
+  <arg name="map_scanmatch_frame_name" default="scanmatcher_frame"/>
+  <arg name="pub_map_odom_transform"   default="true"/>
+  <arg name="scan_queue_size"          default="10"/>
+  <arg name="base_frame" default="/base_link" />
+  <arg name="odom_frame" default="/odom" />
+  <arg name="map_frame" default="/map" />
+  <arg name="scan_topic" default="/scan" />
+  <arg name="map_size"   default="512" />
+
+  <!-- startup ydlidar -->
+  <include file="$(find ydlidar)/launch/lidar.launch" />
+
+  <node pkg="hector_mapping" type="hector_mapping" name="hector_mapping" output="screen">
+    <!-- Frame names -->
+    <param name="map_frame" value="$(arg map_frame)" />
+    <param name="base_frame" value="$(arg base_frame)" />
+    <param name="odom_frame" value="$(arg odom_frame)" />
+
+    <!-- Map resolution/size/start point -->
+    <param name="map_resolution" value="0.04"/>
+    <param name="map_size"       value="$(arg map_size)"/>
+    <param name="map_start_x"    value="0.2" />
+    <param name="map_start_y"    value="0.2" />
+
+    <!-- Map update parameters -->
+    <param name="map_update_distance_thresh" value="0.45" />
+    <param name="map_update_angle_thresh"    value="0.30" />
+    <param name="map_pub_period"             value="2.2" />
+    <param name="map_multi_res_levels"       value="2" />
+    <param name="update_factor_free"         value="0.4" />
+    <param name="update_factor_occupied"     value="0.9" />
+
+    <!-- lidar laser parameters -->
+    <param name="scan_topic"         value="$(arg scan_topic)" />
+    <param name="laser_min_dist"     value="0.06" />
+    <param name="laser_max_dist"     value="4.09" />
+    <param name="laser_z_min_value"  value="-1.0" />
+    <param name="laser_z_max_value"  value="1.0" />
+
+    <param name="output_timing"              value="false"/>
+    <param name="scan_subscriber_queue_size" value="$(arg scan_queue_size)"/>
+    <param name="pub_map_odom_transform"     value="$(arg pub_map_odom_transform)"/>
+
+    <param name="advertise_map_service"      value="true" />
+    <param name="use_tf_scan_transformation" value="true" />
+    <param name="use_tf_pose_start_estimate" value="true" />
+    <param name="tf_map_scanmatch_transform_frame_name" value="$(arg map_scanmatch_frame_name)" />
+  </node>
+
+  <node pkg="tf" type="static_transform_publisher" name="map_to_odom" args="0.0 0.0 0.0 0 0 0.0 /map /odom 100"/>
+</launch>
+

src/robot_navigation/launch/hector_mapping_view.launch

@@ -0,0 +1,4 @@
+<launch>
+   <!-- startup rviz to show hector mapping process -->
+  <node name="rviz" type="rviz" pkg="rviz" args="-d $(find robot_navigation)/rviz/hector_view.rviz" />
+</launch>

src/robot_navigation/launch/move_base.launch

@@ -0,0 +1,43 @@
+<!--
+  Copyright(c): 2016-2020 ROS小课堂 www.corvin.cn
+  Description:
+    move base package launch file, load config file for slam,config file list below:
+    (1)costmap_common_params.yaml -global_costmap
+    (2)costmap_common_params.yaml -local_costmap
+    (3)local_costmap_params.yaml
+    (4)global_costmap_params.yaml
+    (5)base_local_planner_params.yaml
+    controller_frequency:move_base向控制底盘移动话题cmd_vel发送控制命令的频率.
+  Author: corvin
+  History:
+    20190728:init this file.
+-->
+<launch>
+  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen" clear_params="true">
+    <param name="controller_frequency" value="15" />
+    <param name="controller_patiente"  value="3.0" />
+
+    <param name="planner_patience"  value="5.0" />
+    <param name="planner_frequency" value="0.0" />
+    <param name="conservative_reset_dist" value="3.0" />
+
+    <param name="recovery_behavior_enabled" value="true" />
+    <param name="clearing_rotation_allowed" value="true" />
+
+    <param name="shutdown_costmaps"    value="false" />
+    <param name="oscillation_timeout"  value="10.0" />
+    <param name="oscillation_distance" value="0.2" />
+
+    <param name="max_planning_retries" value="10"  />
+
+    <rosparam file="$(find robot_navigation)/config/costmap_common_params.yaml" command="load" ns="global_costmap" />
+    <rosparam file="$(find robot_navigation)/config/costmap_common_params.yaml" command="load" ns="local_costmap" />
+    <rosparam file="$(find robot_navigation)/config/local_costmap_params.yaml"  command="load" />
+    <rosparam file="$(find robot_navigation)/config/global_costmap_params.yaml" command="load" />
+
+    <!-- 加载move_base使用的全局路径规划和局部路径规划算法及对应的参数  -->
+    <rosparam file="$(find robot_navigation)/config/base_global_planner_params.yaml" command="load" />
+    <rosparam file="$(find robot_navigation)/config/dwa_local_planner_params.yaml" command="load" />
+  </node>
+</launch>
+

src/robot_navigation/launch/rplidar.launch

@@ -0,0 +1,10 @@
+<launch>
+  <node name="rplidarNode"  pkg="rplidar_ros"  type="rplidarNode" output="screen">
+	  <param name="serial_port"      type="string" value="/dev/ttyUSB0"/>
+	  <param name="serial_baudrate"  type="int"    value="115200"/><!--A1/A2 -->
+	  <!--param name="serial_baudrate"   type="int"   value="256000"--><!--A3 -->
+	  <param name="frame_id"          type="string" value="lidar"/>
+	  <param name="inverted"          type="bool"   value="false"/>
+	  <param name="angle_compensate"  type="bool"   value="true"/>
+  </node>
+</launch>

src/robot_navigation/launch/view_model.launch

@@ -0,0 +1,7 @@
+<launch>
+  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
+    <param name="use_gui" value="true"/>
+  </node>
+  
+  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find carebot_navigation)/rviz/model.rviz" />
+</launch>

src/robot_navigation/launch/view_navigation.launch

@@ -0,0 +1,6 @@
+<!--
+  Used for visualising the robot while building a map or navigating with the ros navistack.
+ -->
+<launch>
+  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find robot_navigation)/rviz/view_navigation.rviz"/>
+</launch>

src/robot_navigation/launch/view_navigation_app.launch

@@ -0,0 +1,9 @@
+<!--
+  Used for visualising the turtlebot while building a map or navigating with the ros navistack.
+  
+  Use this version of view_navigation.launch whenever the navigation related topics lay inside
+  an application namespace (that is, navigation was launched by the app manager).
+ -->
+<launch>
+  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find turtlebot_rviz_launchers)/rviz/navigation_app.rviz"/>
+</launch>

src/robot_navigation/launch/view_teleop_navigation.launch

@@ -0,0 +1,7 @@
+<launch>
+  <!-- turtlebot_teleop_key already has its own built in velocity smoother -->
+  <node pkg="teleop_twist_keyboard" type="teleop_twist_keyboard.py" name="teleop_keyboard"  output="screen">
+  </node>
+
+  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find navigation)/rviz/navigation.rviz"/>
+</launch>

src/robot_navigation/launch/ydlidar_X2L.launch

@@ -0,0 +1,29 @@
+<!--
+  Copyright: 2016-2020 www.corvin.cn ROS小课堂
+  Description:加载启动EAI的X2L雷达.
+  Author: corvin
+  History:
+    20200325: init this file.
+    20200927:在启动雷达时候配置了required参数,当雷达没有正常启动,则整个导航过程停止.
+-->
+<launch>
+  <node name="ydlidar_X2L_node" pkg="ydlidar_ros" type="ydlidar_node" output="screen" required="true" >
+    <param name="port"      type="string" value="/dev/ydlidar"/>
+    <param name="baudrate"  type="int"    value="115200"/>
+    <param name="frame_id"  type="string" value="lidar"/>
+
+    <param name="resolution_fixed" type="bool"  value="true"/>
+    <param name="auto_reconnect"   type="bool"  value="true"/>
+
+    <param name="reversion"    type="bool"   value="false"/>
+    <param name="angle_min"    type="double" value="-180" />
+    <param name="angle_max"    type="double" value="180" />
+    <param name="range_min"    type="double" value="0.1" />
+    <param name="range_max"    type="double" value="8.0" />
+    <param name="ignore_array" type="string" value="" />
+
+    <param name="frequency"    type="double" value="7"/>
+    <param name="samp_rate"    type="int"    value="3"/>
+    <param name="isSingleChannel" type="bool"  value="true"/>
+  </node>
+</launch>

src/robot_navigation/maps/blank_map.yaml

@@ -0,0 +1,6 @@
+image: blank_map.pgm
+resolution: 0.01
+origin: [-2.5, -2.5, 0]
+occupied_thresh: 0.65
+free_thresh: 0.196 # Taken from the Willow Garage map in the turtlebot_navigation package
+negate: 0

src/robot_navigation/maps/blank_map_with_obstacle.yaml

@@ -0,0 +1,6 @@
+image: blank_map_with_obstacle.pgm
+resolution: 0.01
+origin: [-2.5, -2.5, 0]
+occupied_thresh: 0.65
+free_thresh: 0.195 # Taken from the Willow Garage map in the turtlebot_navigation package
+negate: 0

src/robot_navigation/maps/mymap.yaml

@@ -0,0 +1,7 @@
+image: mymap.pgm
+resolution: 0.020000
+origin: [-5.000000, -5.000000, 0.000000]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196
+

src/robot_navigation/maps/mymap_org.yaml

@@ -0,0 +1,7 @@
+image: mymap.pgm
+resolution: 0.020000
+origin: [-5.000000, -5.000000, 0.000000]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196
+

src/robot_navigation/maps/test.yaml

@@ -0,0 +1,7 @@
+image: test.pgm
+resolution: 0.020000
+origin: [-15.240000, -15.880000, 0.000000]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196
+

src/robot_navigation/nodes/position_nav.py

@@ -0,0 +1,187 @@
+#!/usr/bin/env python  
+  
+################################################
+# Copyright(c): 2016-2018 www.corvin.cn
+################################################
+# Author: corvin
+################################################
+# Description:
+#   four destination postion auto navigation.
+################################################
+# History:
+#   20180413: init this file.
+################################################
+
+import rospy  
+import actionlib  
+from actionlib_msgs.msg import *  
+from geometry_msgs.msg import Pose, PoseWithCovarianceStamped, Point, Quaternion, Twist  
+from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal  
+from random import sample  
+from math import pow, sqrt  
+  
+class PositionNav():  
+    def __init__(self):  
+        rospy.init_node('position_nav_node', anonymous=True)  
+        rospy.on_shutdown(self.shutdown)  
+          
+        # How long in seconds should the robot pause at each location?  
+        self.rest_time = rospy.get_param("~rest_time", 10)  
+          
+        # Are we running in the fake simulator?  
+        self.fake_test = rospy.get_param("~fake_test", False)  
+          
+        # Goal state return values  
+        goal_states = ['PENDING', 'ACTIVE', 'PREEMPTED',   
+                       'SUCCEEDED', 'ABORTED', 'REJECTED',  
+                       'PREEMPTING', 'RECALLING', 'RECALLED',  
+                       'LOST']  
+          
+        # Set up the goal locations. Poses are defined in the map frame.
+        # An easy way to find the pose coordinates is to point-and-click  
+        # Nav Goals in RViz when running in the simulator.  
+        #  
+        # Pose coordinates are then displayed in the terminal  
+        # that was used to launch RViz.  
+        locations = dict()  
+        locations['one'] = Pose(Point(-4.600, -2.583, 0.000), Quaternion(0.000, 0.000, 0.000, 0.999))  
+        locations['two'] = Pose(Point(-3.758, -2.493, 0.000), Quaternion(0.000, 0.000, 0.694, 0.720))  
+        locations['three'] = Pose(Point(-3.659, 0.441, 0.000), Quaternion(0.000, 0.000, 0.999, -0.039))  
+        locations['four'] = Pose(Point(-4.613, 0.365, 0.000), Quaternion(0.000, 0.000, -0.719, 0.695))  
+          
+        # Publisher to manually control the robot (e.g. to stop it)  
+        self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=10)  
+          
+        # Subscribe to the move_base action server  
+        self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)  
+        rospy.loginfo("Waiting for move_base action server...")  
+          
+        # Wait 60 seconds for the action server to become available  
+        self.move_base.wait_for_server(rospy.Duration(60))  
+          
+        rospy.loginfo("Connected to move base server")  
+          
+        # A variable to hold the initial pose of the robot to be set by   
+        # the user in RViz  
+        initial_pose = PoseWithCovarianceStamped()  
+          
+        # Variables to keep track of success rate, running time,  
+        # and distance traveled  
+        n_locations = len(locations)  
+        n_goals = 0  
+        n_successes = 0  
+        i = 0  
+        distance_traveled = 0  
+        start_time = rospy.Time.now()  
+        running_time = 0  
+        location = ""  
+        last_location = ""  
+        sequeue=['one', 'two', 'three', 'four'] 
+          
+        # Get the initial pose from the user  
+        rospy.loginfo("*** Click the 2D Pose Estimate button in RViz to set the robot's initial pose...")  
+        rospy.wait_for_message('initialpose', PoseWithCovarianceStamped)  
+        self.last_location = Pose()  
+        rospy.Subscriber('initialpose', PoseWithCovarianceStamped, self.update_initial_pose)  
+          
+        # Make sure we have the initial pose  
+        while initial_pose.header.stamp == "":  
+            rospy.sleep(1)  
+              
+        rospy.loginfo("Starting position navigation ")  
+          
+        # Begin the main loop and run through a sequence of locations  
+        while not rospy.is_shutdown():  
+            # If we've gone through the all sequence, then exit
+            if i == n_locations:  
+              rospy.logwarn("Now reach all destination, now exit...")
+              rospy.signal_shutdown('Quit')  
+              break
+              
+            # Get the next location in the current sequence  
+            location = sequeue[i]  
+                          
+            # Keep track of the distance traveled.  
+            # Use updated initial pose if available.  
+            if initial_pose.header.stamp == "":  
+                distance = sqrt(pow(locations[location].position.x -   
+                                    locations[last_location].position.x, 2) +  
+                                pow(locations[location].position.y -   
+                                    locations[last_location].position.y, 2))  
+            else:  
+                rospy.loginfo("Updating current pose.")  
+                distance = sqrt(pow(locations[location].position.x -   
+                                    initial_pose.pose.pose.position.x, 2) +  
+                                pow(locations[location].position.y -   
+                                    initial_pose.pose.pose.position.y, 2))  
+                initial_pose.header.stamp = ""  
+              
+            # Store the last location for distance calculations  
+            last_location = location  
+              
+            # Increment the counters  
+            i += 1  
+            n_goals += 1  
+          
+            # Set up the next goal location  
+            self.goal = MoveBaseGoal()
+            self.goal.target_pose.pose = locations[location]  
+            self.goal.target_pose.header.frame_id = 'map'  
+            self.goal.target_pose.header.stamp = rospy.Time.now()  
+              
+            # Let the user know where the robot is going next  
+            rospy.loginfo("Going to: " + str(location))  
+              
+            # Start the robot toward the next location  
+            self.move_base.send_goal(self.goal) #move_base.send_goal()  
+              
+            # Allow 5 minutes to get there  
+            finished_within_time = self.move_base.wait_for_result(rospy.Duration(300))   
+            # Check for success or failure  
+            if not finished_within_time:  
+                self.move_base.cancel_goal() #move_base.cancle_goal()  
+                rospy.loginfo("Timed out achieving goal")  
+            else:  
+                state = self.move_base.get_state() #move_base.get_state()  
+                if state == GoalStatus.SUCCEEDED:  
+                    rospy.loginfo("Goal succeeded!")  
+                    n_successes += 1  
+                    distance_traveled += distance  
+                    rospy.loginfo("State:" + str(state))  
+                else:  
+                  rospy.loginfo("Goal failed with error code: " + str(goal_states[state]))  
+              
+            # How long have we been running?  
+            running_time = rospy.Time.now() - start_time  
+            running_time = running_time.secs / 60.0  
+              
+            # Print a summary success/failure, distance traveled and time elapsed  
+            rospy.loginfo("Success so far: " + str(n_successes) + "/" +   
+                          str(n_goals) + " = " +   
+                          str(100 * n_successes/n_goals) + "%")  
+            rospy.loginfo("Running time: " + str(trunc(running_time, 1)) +   
+                          " min Distance: " + str(trunc(distance_traveled, 1)) + " m")  
+            rospy.sleep(self.rest_time)  
+      
+    def update_initial_pose(self, initial_pose):  
+        self.initial_pose = initial_pose  
+  
+    def shutdown(self):  
+        rospy.loginfo("Stopping the robot...")  
+        self.move_base.cancel_goal()  
+        rospy.sleep(2)  
+        self.cmd_vel_pub.publish(Twist())  
+        rospy.sleep(1)  
+        
+def trunc(f, n):  
+    # Truncates/pads a float f to n decimal places without rounding  
+    slen = len('%.*f' % (n, f))  
+    return float(str(f)[:slen])  
+  
+if __name__ == '__main__':  
+    try:  
+        PositionNav()  
+        rospy.spin()  
+    except rospy.ROSInterruptException:  
+        rospy.loginfo("AMCL position navigation finished.")  
+

src/robot_navigation/package.xml

@@ -0,0 +1,45 @@
+<?xml version="1.0"?>
+<package>
+  <name>robot_navigation</name>
+  <version>0.0.0</version>
+  <description>The robot navigation package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="corvin_zhang@corvin.cn">corvin</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, 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 multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/navigation</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <run_depend>roscpp</run_depend>
+  <run_depend>rospy</run_depend>
+  <run_depend>std_msgs</run_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

src/robot_navigation/rviz/blind_nav.rviz

@@ -0,0 +1,367 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /TF1/Frames1
+        - /TF1/Tree1
+        - /Global Map1/Map1
+        - /Bumper/cliff PointCloud1/Status1
+      Splitter Ratio: 0.5
+    Tree Height: 769
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+    Name: Tool Properties
+    Splitter Ratio: 0.588679
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.03
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 1
+      Class: rviz/RobotModel
+      Collision Enabled: false
+      Enabled: true
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        Link Tree Style: Links in Alphabetic Order
+        base_footprint:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        camera_depth_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_depth_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        camera_rgb_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_rgb_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        caster_back_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        caster_front_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        cliff_sensor_front_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        cliff_sensor_left_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        cliff_sensor_right_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        gyro_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        plate_bottom_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        plate_middle_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        plate_top_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        pole_bottom_0_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        pole_bottom_1_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        pole_bottom_2_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        pole_bottom_3_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        pole_bottom_4_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        pole_bottom_5_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        pole_kinect_0_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        pole_kinect_1_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        pole_middle_0_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        pole_middle_1_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        pole_middle_2_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        pole_middle_3_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        pole_top_0_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        pole_top_1_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        pole_top_2_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        pole_top_3_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        wheel_left_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        wheel_right_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+      Name: RobotModel
+      Robot Description: robot_description
+      TF Prefix: ""
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+    - Class: rviz/TF
+      Enabled: false
+      Frame Timeout: 15
+      Frames:
+        All Enabled: false
+      Marker Scale: 1
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: false
+      Tree:
+        {}
+      Update Interval: 0
+      Value: false
+    - Class: rviz/Group
+      Displays:
+        - Alpha: 0.7
+          Class: rviz/Map
+          Color Scheme: costmap
+          Draw Behind: false
+          Enabled: true
+          Name: Costmap
+          Topic: /move_base/local_costmap/costmap
+          Value: true
+        - Alpha: 1
+          Buffer Length: 1
+          Class: rviz/Path
+          Color: 0; 12; 255
+          Enabled: true
+          Name: Planner
+          Topic: /move_base/TrajectoryPlannerROS/local_plan
+          Value: true
+      Enabled: true
+      Name: Local Map
+    - Class: rviz/Group
+      Displays:
+        - Alpha: 0.4
+          Class: rviz/Map
+          Color Scheme: costmap
+          Draw Behind: true
+          Enabled: true
+          Name: Map
+          Topic: /move_base/global_costmap/costmap
+          Value: true
+        - Alpha: 1
+          Buffer Length: 1
+          Class: rviz/Path
+          Color: 255; 0; 0
+          Enabled: true
+          Name: Planner
+          Topic: /move_base/TrajectoryPlannerROS/global_plan
+          Value: true
+      Enabled: true
+      Name: Global Map
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: ""
+      Decay Time: 0
+      Enabled: true
+      Max Color: 255; 255; 255
+      Max Intensity: 4096
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: Bumper/cliff PointCloud
+      Position Transformer: ""
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.05
+      Style: Spheres
+      Topic: /mobile_base/sensors/bumper_pointcloud
+      Use Fixed Frame: true
+      Use rainbow: false
+      Value: true
+    - Alpha: 1
+      Class: rviz/Polygon
+      Color: 25; 255; 0
+      Enabled: true
+      Name: Polygon
+      Topic: /move_base/local_costmap/footprint_layer/footprint_stamped
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: odom
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/MoveCamera
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/Select
+    - Class: rviz/SetInitialPose
+      Topic: /initialpose
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/Measure
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 10.0367
+      Focal Point:
+        X: 0.644818
+        Y: 3.7827
+        Z: -0.57592
+      Name: Current View
+      Near Clip Distance: 0.01
+      Pitch: 1.4398
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 2.99003
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 985
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd0000000400000000000003600000038bfc0200000005fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006901000005fb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000320000038b000000d701000005000000010000010b0000038bfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007301000000320000038b000000a901000005fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a00000024c01000005fb0000000800540069006d006501000000000000045000000000000000000000030f0000038b00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1920
+  X: 0
+  Y: 40

src/robot_navigation/rviz/hector_view.rviz

@@ -0,0 +1,232 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded: ~
+      Splitter Ratio: 0.5
+    Tree Height: 595
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.588679016
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+Toolbars:
+  toolButtonStyle: 2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.0299999993
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 6
+        Y: 6
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 30
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 0.699999988
+      Class: rviz/Map
+      Color Scheme: costmap
+      Draw Behind: false
+      Enabled: true
+      Name: Map
+      Topic: /map
+      Unreliable: false
+      Use Timestamp: false
+      Value: true
+    - Alpha: 1
+      Axes Length: 0.0299999993
+      Axes Radius: 0.0500000007
+      Class: rviz/Pose
+      Color: 255; 25; 0
+      Enabled: true
+      Head Length: 0.300000012
+      Head Radius: 0.100000001
+      Name: Pose
+      Shaft Length: 1
+      Shaft Radius: 0.0500000007
+      Shape: Axes
+      Topic: /slam_out_pose
+      Unreliable: false
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/LaserScan
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 4096
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: LaserScan
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.00999999978
+      Style: Flat Squares
+      Topic: /robot0/laser_0
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Class: rviz/TF
+      Enabled: true
+      Frame Timeout: 15
+      Frames:
+        All Enabled: false
+        base_footprint:
+          Value: true
+        base_link:
+          Value: true
+        lidar:
+          Value: true
+        map:
+          Value: true
+        odom:
+          Value: true
+        scanmatcher_frame:
+          Value: true
+      Marker Scale: 1
+      Name: TF
+      Show Arrows: false
+      Show Axes: true
+      Show Names: true
+      Tree:
+        map:
+          odom:
+            base_footprint:
+              base_link:
+                lidar:
+                  {}
+          scanmatcher_frame:
+            {}
+      Update Interval: 0
+      Value: true
+    - Alpha: 1
+      Class: rviz/RobotModel
+      Collision Enabled: false
+      Enabled: true
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        Link Tree Style: Links in Alphabetic Order
+        base_footprint:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        lidar:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+      Name: RobotModel
+      Robot Description: robot_description
+      TF Prefix: ""
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: odom
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Topic: /initialpose
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 8.82278156
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.0599999987
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 4.02420664
+        Y: -0.984699249
+        Z: -4.72999859
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.0500000007
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.00999999978
+      Pitch: 0.904760242
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 3.01358628
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 876
+  Hide Left Dock: false
+  Hide Right Dock: true
+  QMainWindow State: 000000ff00000000fd00000004000000000000016a000002e2fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000002e2000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000396000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006400000003efc0100000002fb0000000800540069006d00650100000000000006400000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000004d0000002e200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: true
+  Width: 1600
+  X: 0
+  Y: 24

src/robot_navigation/rviz/model.rviz

@@ -0,0 +1,228 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /Grid1
+        - /RobotModel1
+      Splitter Ratio: 0.5
+    Tree Height: 673
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+    Name: Tool Properties
+    Splitter Ratio: 0.588679
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Name: Time
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.03
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        Value: 0; 0; 0
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 1
+      Class: rviz/RobotModel
+      Collision Enabled: false
+      Enabled: true
+      Links:
+        base_footprint:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        camera_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        caster_back_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        caster_front_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        plate_bottom_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        plate_middle_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        plate_top_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        pole_bottom_0_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        pole_bottom_1_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        pole_bottom_2_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        pole_bottom_3_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        pole_bottom_4_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        pole_bottom_5_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        pole_kinect_0_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        pole_kinect_1_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        pole_middle_0_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        pole_middle_1_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        pole_middle_2_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        pole_middle_3_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        pole_top_0_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        pole_top_1_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        pole_top_2_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        pole_top_3_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        wheel_left_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        wheel_right_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+      Name: RobotModel
+      Robot Description: robot_description
+      TF Prefix: ""
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: /base_footprint
+  Name: root
+  Tools:
+    - Class: rviz/MoveCamera
+    - Class: rviz/Interact
+    - Class: rviz/Select
+    - Class: rviz/SetInitialPose
+      Topic: /initialpose
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 1.18557
+      Focal Point:
+        Value: 0; 0; 0
+        X: 0
+        Y: 0
+        Z: 0
+      Name: Current View
+      Near Clip Distance: 0.01
+      Pitch: 0.490399
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 5.71357
+    Saved: ~
+Window Geometry:
+  Height: 916
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd00000004000000000000018c0000032bfc0200000005fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006901000005fb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000320000032b000000d701000005000000010000010b0000032bfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007301000000320000032b000000a901000005fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a0000001eb01000005fb0000000800540069006d00650100000000000004500000000000000000000002f30000032b00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Width: 1428
+  X: 56
+  Y: -31

src/robot_navigation/rviz/navigation_app.rviz

@@ -0,0 +1,405 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /TF1/Frames1
+        - /TF1/Tree1
+        - /Global Map1/Costmap1
+        - /Global Map1/Planner1
+        - /Local Map1/Costmap1
+        - /Local Map1/Planner1
+      Splitter Ratio: 0.5
+    Tree Height: 762
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+    Name: Tool Properties
+    Splitter Ratio: 0.588679
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: LaserScan
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.03
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 1
+      Class: rviz/RobotModel
+      Collision Enabled: false
+      Enabled: true
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        Link Tree Style: Links in Alphabetic Order
+        base_footprint:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        camera_depth_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_depth_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        camera_rgb_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_rgb_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        caster_back_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        caster_front_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        cliff_sensor_front_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        cliff_sensor_left_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        cliff_sensor_right_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        gyro_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        plate_bottom_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        plate_middle_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        plate_top_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        pole_bottom_0_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        pole_bottom_1_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        pole_bottom_2_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        pole_bottom_3_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        pole_bottom_4_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        pole_bottom_5_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        pole_kinect_0_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        pole_kinect_1_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        pole_middle_0_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        pole_middle_1_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        pole_middle_2_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        pole_middle_3_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        pole_top_0_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        pole_top_1_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        pole_top_2_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        pole_top_3_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        wheel_left_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        wheel_right_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+      Name: RobotModel
+      Robot Description: robot_description
+      TF Prefix: ""
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+    - Class: rviz/TF
+      Enabled: false
+      Frame Timeout: 15
+      Frames:
+        All Enabled: false
+      Marker Scale: 1
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: false
+      Tree:
+        {}
+      Update Interval: 0
+      Value: false
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/LaserScan
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0
+      Enabled: true
+      Max Color: 255; 255; 255
+      Max Intensity: 4096
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: LaserScan
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.01
+      Style: Squares
+      Topic: /scan
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/PointCloud2
+      Color: 255; 0; 0
+      Color Transformer: FlatColor
+      Decay Time: 0
+      Enabled: true
+      Max Color: 255; 255; 255
+      Max Intensity: 4096
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: Bumper Hit
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.08
+      Style: Spheres
+      Topic: /mobile_base/sensors/bumper_pointcloud
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Alpha: 0.7
+      Class: rviz/Map
+      Color Scheme: map
+      Draw Behind: false
+      Enabled: true
+      Name: Map
+      Topic: /app_manager/application/map
+      Value: true
+    - Class: rviz/Group
+      Displays:
+        - Alpha: 0.7
+          Class: rviz/Map
+          Color Scheme: costmap
+          Draw Behind: true
+          Enabled: true
+          Name: Costmap
+          Topic: /app_manager/application/move_base/global_costmap/costmap
+          Value: true
+        - Alpha: 1
+          Buffer Length: 1
+          Class: rviz/Path
+          Color: 255; 0; 0
+          Enabled: false
+          Name: Planner
+          Topic: /app_manager/application/move_base/TrajectoryPlannerROS/global_plan
+          Value: false
+      Enabled: true
+      Name: Global Map
+    - Class: rviz/Group
+      Displays:
+        - Alpha: 0.7
+          Class: rviz/Map
+          Color Scheme: costmap
+          Draw Behind: false
+          Enabled: true
+          Name: Costmap
+          Topic: /app_manager/application/move_base/local_costmap/costmap
+          Value: true
+        - Alpha: 1
+          Buffer Length: 1
+          Class: rviz/Path
+          Color: 0; 12; 255
+          Enabled: true
+          Name: Planner
+          Topic: /app_manager/application/move_base/TrajectoryPlannerROS/local_plan
+          Value: true
+      Enabled: true
+      Name: Local Map
+    - Arrow Length: 0.2
+      Class: rviz/PoseArray
+      Color: 0; 192; 0
+      Enabled: true
+      Name: Amcl Particles
+      Topic: /particlecloud
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: map
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/MoveCamera
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/Select
+    - Class: rviz/SetInitialPose
+      Topic: /initialpose
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/Measure
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 19.5252
+      Focal Point:
+        X: 0.359644
+        Y: 0.951594
+        Z: -0.307131
+      Name: Current View
+      Near Clip Distance: 0.01
+      Pitch: 1.5698
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 3.34502
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 978
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd00000004000000000000026b00000384fc0200000005fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006901000005fb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003200000384000000d701000005000000010000010b00000384fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003200000384000000a901000005fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a00000024c01000005fb0000000800540069006d00650100000000000004500000000000000000000003140000038400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1680
+  X: 0
+  Y: 40

src/robot_navigation/rviz/readme.txt

@@ -0,0 +1,5 @@
+
+===== Filename Extensions =====
+
+ * .vcg  : fuerte
+ * .rviz : groovy

src/robot_navigation/rviz/view_navigation.rviz

@@ -0,0 +1,331 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 123
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Odometry1/Shape1
+      Splitter Ratio: 0.486111104
+    Tree Height: 314
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.588679016
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: LaserScan
+Toolbars:
+  toolButtonStyle: 2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.699999988
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 50; 115; 54
+      Enabled: true
+      Line Style:
+        Line Width: 0.0299999993
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 100
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Class: rviz/TF
+      Enabled: true
+      Frame Timeout: 15
+      Frames:
+        All Enabled: true
+        base_footprint:
+          Value: true
+        base_imu_link:
+          Value: true
+        base_link:
+          Value: true
+        lidar:
+          Value: true
+        map:
+          Value: true
+        odom_combined:
+          Value: true
+      Marker Scale: 0.600000024
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: true
+      Tree:
+        map:
+          odom_combined:
+            base_footprint:
+              base_link:
+                base_imu_link:
+                  {}
+                lidar:
+                  {}
+      Update Interval: 0
+      Value: true
+    - Alpha: 0.800000012
+      Class: rviz/Map
+      Color Scheme: map
+      Draw Behind: false
+      Enabled: true
+      Name: Map
+      Topic: /map
+      Unreliable: false
+      Use Timestamp: false
+      Value: true
+    - Alpha: 0.600000024
+      Class: rviz/Map
+      Color Scheme: costmap
+      Draw Behind: false
+      Enabled: true
+      Name: Map
+      Topic: /move_base/global_costmap/costmap
+      Unreliable: false
+      Use Timestamp: false
+      Value: true
+    - Alpha: 0.5
+      Class: rviz/Map
+      Color Scheme: map
+      Draw Behind: false
+      Enabled: true
+      Name: Map
+      Topic: /move_base/local_costmap/costmap
+      Unreliable: false
+      Use Timestamp: false
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/LaserScan
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 1016
+      Min Color: 0; 0; 0
+      Min Intensity: 1008
+      Name: LaserScan
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.00999999978
+      Style: Flat Squares
+      Topic: /scan
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Alpha: 1
+      Class: rviz/RobotModel
+      Collision Enabled: false
+      Enabled: true
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        Link Tree Style: Links in Alphabetic Order
+        base_footprint:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        base_imu_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        lidar:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+      Name: RobotModel
+      Robot Description: robot_description
+      TF Prefix: ""
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+    - Alpha: 1
+      Buffer Length: 1
+      Class: rviz/Path
+      Color: 44; 65; 255
+      Enabled: true
+      Head Diameter: 0.300000012
+      Head Length: 0.200000003
+      Length: 0.300000012
+      Line Style: Lines
+      Line Width: 0.0299999993
+      Name: Path
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Pose Color: 255; 85; 255
+      Pose Style: None
+      Radius: 0.0299999993
+      Shaft Diameter: 0.100000001
+      Shaft Length: 0.100000001
+      Topic: /move_base/DWAPlannerROS/global_plan
+      Unreliable: false
+      Value: true
+    - Alpha: 1
+      Buffer Length: 1
+      Class: rviz/Path
+      Color: 255; 29; 218
+      Enabled: true
+      Head Diameter: 0.300000012
+      Head Length: 0.200000003
+      Length: 0.300000012
+      Line Style: Lines
+      Line Width: 0.0299999993
+      Name: Path
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Pose Color: 255; 85; 255
+      Pose Style: None
+      Radius: 0.0299999993
+      Shaft Diameter: 0.100000001
+      Shaft Length: 0.100000001
+      Topic: /move_base/DWAPlannerROS/local_plan
+      Unreliable: false
+      Value: true
+    - Alpha: 1
+      Class: rviz/Polygon
+      Color: 0; 0; 255
+      Enabled: true
+      Name: Polygon
+      Topic: /move_base/global_costmap/footprint
+      Unreliable: false
+      Value: true
+    - Angle Tolerance: 0.0500000007
+      Class: rviz/Odometry
+      Covariance:
+        Orientation:
+          Alpha: 0.5
+          Color: 255; 255; 127
+          Color Style: Unique
+          Frame: Local
+          Offset: 1
+          Scale: 1
+          Value: true
+        Position:
+          Alpha: 0.300000012
+          Color: 204; 51; 204
+          Scale: 1
+          Value: true
+        Value: false
+      Enabled: true
+      Keep: 100
+      Name: Odometry
+      Position Tolerance: 0.0500000007
+      Shape:
+        Alpha: 0.5
+        Axes Length: 1
+        Axes Radius: 0.100000001
+        Color: 255; 0; 0
+        Head Length: 0.0500000007
+        Head Radius: 0.0500000007
+        Shaft Length: 0.0500000007
+        Shaft Radius: 0.0199999996
+        Value: Arrow
+      Topic: /odom_ekf
+      Unreliable: false
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 45; 32; 41
+    Default Light: true
+    Fixed Frame: map
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Topic: /initialpose
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 4.64580727
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.0599999987
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 1.49610639
+        Y: -0.156017661
+        Z: 0.169606268
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.0500000007
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.00999999978
+      Pitch: 1.1997968
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 4.7037096
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: true
+  Height: 1056
+  Hide Left Dock: true
+  Hide Right Dock: true
+  QMainWindow State: 000000ff00000000fd00000004000000000000016a00000396fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000002800000396000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002e2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002e2000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650100000000000007800000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000007800000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: true
+  Width: 1920
+  X: 1600
+  Y: 24

src/ros_arduino_bridge/ros_arduino_python/src/ros_arduino_python/base_controller.py

@@ -238,9 +238,10 @@ class BaseController:
         va = dist_A/dt
         vb = dist_B/dt
 
+
         #正向运动学模型,计算里程计模型
-        vx  = (-va + vb)/2.0
-        vth = (-va - vb)/(self.wheel_track)
+        vx  = (va + vb)/2.0
+        vth = (va - vb)/(self.wheel_track)
 
         delta_x  = (vx*cos(self.th))*dt
         delta_y  = (vx*sin(self.th))*dt