ROS入門(七)——Rviz、建圖導航與存儲地圖

本文轉載自查看原文 2020-05-03 12:09 3768 ROS/ 其他

ROS入門(七)——Rviz的使用與建圖導航
　　iwehdio的博客園：https://www.cnblogs.com/iwehdio/

1、Rviz

Rviz可以與實物或者Gazebo中的模型進行聯合使用。在https://www.cnblogs.com/iwehdio/p/12763361.html中，我們看到，機器人模型可以在Rviz中顯示，而且相機的圖像、激光雷達的掃描結果也能顯示。
那我們就想要，在Rviz中對https://www.cnblogs.com/iwehdio/p/12774226.html中自建小車進行顯示。
使用Gazebo打開仿真模型后，如果直接打開Rviz，添加話題或者可視化組件，都會報錯而無法顯示。如報錯小車車體 “No transform from [chassis] to [map]”。這是因為Rviz沒有收到車體的坐標轉換信息即 tf。
不妨從RRbot是怎樣做到在Rviz中顯示的看起。啟動RRbot示例，可以看到，圖像和激光雷達以及機器人模型都是正常顯示的：
查看坐標轉換 tf 中包含了那些內容：
```
$ rostopic echo /tf
$ rostopic echo /tf_static
```
/tf 話題下，發布了link直接的坐標轉換關系。

/tf_static 話題下，發布了機器人與世界、相機、激光雷達之間的坐標轉換關系。
更清楚的，可以看一下tf樹的結構：
```
$ rosrun rqt_tf_tree rqt_tf_tree
```
即坐標轉換使得機器人的每個組件，最終都能查詢到各自之間的坐標轉換關系。
回到我們的自建小車上來，同樣的，啟動Gazebo模型，再查詢tf樹：

可以看到，機器人內部的tf都是構造好了的。
但是打開Rviz后，這些話題都不見了。
查找原因，注意到上圖的tf樹中，除了odom是/gazebo發布的，其他都是由/robot_state_publisher發布的。這是由於，啟動mycar_world.launch時，同時也include了mycar_control.launch文件，在這個文件中啟動了/robot_state_publisher節點。而mycar_rviz.launch中又一次啟動了/robot_state_publisher節點，覆蓋了之前的發布。

所以，我們要做的就是，發布/map與/odom的tf，刪除並且不再啟動/robot_state_publisher節點。

<!-- 加入 -->
<node pkg="tf" type="static_transform_publisher" name="odem_broadcaster" args="0 0 0 0 0 0 1 map odom 50" /> 
<!-- 刪除 -->
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/>

再次打開Rviz，添加RobotModel、Odometry、Camera和LaserScan，並且保存配置。在mycar_rviz.launch中加入：
```

  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find mycar_description)/launch/mycar.rviz"/>
```
啟動Rviz配置文件，即可在Rviz中顯示。

2、建圖與導航

在ROS中，進行導航需要使用到的三個包是：
（1） move_base：根據參照的消息進行路徑規划，使移動機器人到達指定的位置；
（2） gmapping：根據激光數據（或者深度數據模擬的激光數據）建立地圖；
（3） amcl：根據已經有的地圖進行定位。
首先是 gmapping，gmapping 訂閱和發布的話題如下：

接收了坐標系之間的轉換關系（odom與base_link）和激光雷達的數據，發布了map與odom之間的坐標轉換，進行機器人的定位和建立的地圖數據。
然后是move_base，它接收了地圖數據等，通過代價地圖等進行路徑規划：
最后是AMCL，通過已有的數據進行定位：

接收了激光雷達、地圖數據和坐標系之間的轉換關系，輸出修正后的定位數據。
gampping 的配置：

gmapping 配置文件：主要是修改最上方的，激光雷達話題、base_link的框架名、odom的框架名。下方的其他數據主要是與實際建圖效果有關的參數配置。

<launch>
  <arg name="scan_topic"  default="mycar/laser/scan" />
  <arg name="base_frame"  default="footprint"/>
  <arg name="odom_frame"  default="odom"/>

  <node pkg="gmapping" type="slam_gmapping" name="slam_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="1.0"/>        
    <param name="maxUrange" value="20.0"/>                
    <param name="maxRange" value="25.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="200"/>
    <param name="srr" value="0.01"/>
    <param name="srt" value="0.02"/>
    <param name="str" value="0.01"/>
    <param name="stt" value="0.02"/>
    <param name="linearUpdate" value="0.5"/>
    <param name="angularUpdate" value="0.436"/>
    <param name="temporalUpdate" value="-1.0"/>
    <param name="resampleThreshold" value="0.5"/>
    <param name="particles" value="80"/>
    <param name="xmin" value="-25.0"/>
    <param name="ymin" value="-25.0"/>
    <param name="xmax" value="25.0"/>
    <param name="ymax" value="25.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"/>
    <remap from="scan" to="$(arg scan_topic)"/>
  </node>
</launch>

啟動gmapping的launch文件。

<launch>
  <include file="$(find mycar_slam)/launch/include/robot_gmapping.launch.xml"/>

 <include file="$(find mycar_navigation)/launch/include/move_base.launch.xml"/>

</launch>

不用move_base也可以用鍵盤控制節點代替。

運行效果：

move_base的配置：

move_base的啟動文件：

<!-- 
    ROS navigation stack with velocity smoother and safety (reactive) controller
-->
<launch>
  <!--include file="$(find navigation_sim_demo)/launch/include/velocity_smoother.launch.xml"/-->
  <!--include file="$(find navigation_sim_demo)/launch/include/safety_controller.launch.xml"/-->
  
  <arg name="odom_frame_id"   default="odom"/>
  <arg name="base_frame_id"   default="footprint"/>
  <arg name="global_frame_id" default="map"/>
  <arg name="odom_topic" default="/odom" />
  <arg name="laser_topic" default="/mycar/laser/scan" />

  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
    <rosparam file="$(find mycar_navigation)/param/costmap_common_params.yaml" command="load" ns="global_costmap" />
    <rosparam file="$(find mycar_navigation)/param/costmap_common_params.yaml" command="load" ns="local_costmap" />   
    <rosparam file="$(find mycar_navigation)/param/local_costmap_params.yaml" command="load" />   
    <rosparam file="$(find mycar_navigation)/param/global_costmap_params.yaml" command="load" />
    <rosparam file="$(find mycar_navigation)/param/dwa_local_planner_params.yaml" command="load" />
    <rosparam file="$(find mycar_navigation)/param/move_base_params.yaml" command="load" />
    <rosparam file="$(find mycar_navigation)/param/global_planner_params.yaml" command="load" />
    <rosparam file="$(find mycar_navigation)/param/navfn_global_planner_params.yaml" command="load" />
    
    <!-- reset frame_id parameters using user input data -->
    <param name="global_costmap/global_frame" value="$(arg global_frame_id)"/>
    <param name="global_costmap/robot_base_frame" value="$(arg base_frame_id)"/>
    <param name="local_costmap/global_frame" value="$(arg odom_frame_id)"/>
    <param name="local_costmap/robot_base_frame" value="$(arg base_frame_id)"/>
    <param name="DWAPlannerROS/global_frame_id" value="$(arg odom_frame_id)"/>

    <remap from="odom" to="$(arg odom_topic)"/>
    <remap from="scan" to="$(arg laser_topic)"/>
    <remap from="cmd_vel" to="/mycar/cmd_vel"/>

  </node>
</launch>

主要是顯示更改odom、map和base_link的框架名，然后更改odom和激光雷達的話題名。下面讀入了move_base的配置文件。最后要主要將鏈接的控制話題進行重映射，連接到本機器人的控制。

move_base的配置文件：

costmap_common_params.yaml：主要是要更改所讀入的傳感器的數據類型和話題，其他參數進行適當調節。

max_obstacle_height: 2.40  # assume something like an arm is mounted on top of the robot

# Obstacle Cost Shaping (http://wiki.ros.org/costmap_2d/hydro/inflation)
robot_radius: 0.25  # distance a circular robot should be clear of the obstacle (kobuki: 0.18)
# footprint: [[x0, y0], [x1, y1], ... [xn, yn]]  # if the robot is not circular

map_type: voxel

voxel_layer:
  enabled:              true
  max_obstacle_height:  2.2
  origin_z:             0.0
  z_resolution:         0.1
  z_voxels:             22
  unknown_threshold:    15
  mark_threshold:       0
  combination_method:   1
  track_unknown_space:  true    #true needed for disabling global path planning through unknown space
  obstacle_range: 2.5
  raytrace_range: 3.0
  publish_voxel_map: true
  observation_sources:  scan 
  scan:
    data_type: LaserScan
    topic: /mycar/laser/scan
    marking: true
    clearing: true
    min_obstacle_height: 0.1
    max_obstacle_height: 0.3

#cost_scaling_factor and inflation_radius were now moved to the inflation_layer ns
inflation_layer:
  enabled:              true
  cost_scaling_factor:  2.58  # exponential rate at which the obstacle cost drops off (default: 10)
  inflation_radius:     1.0  # max. distance from an obstacle at which costs are incurred for planning paths.

static_layer:
  enabled:              true

dwa_local_planner_params.yaml：對參數進行適當調節。

DWAPlannerROS:

# Robot Configuration Parameters
  max_vel_x: 0.5
  min_vel_x: 0.0

  max_vel_y: 0.0
  min_vel_y: 0.0

  # The velocity when robot is moving in a straight line
  max_trans_vel:  0.55
  min_trans_vel:  0.1
  trans_stopped_vel: 0.1

  max_rot_vel: 5.0
  min_rot_vel: 0.8
  rot_stopped_vel: 0.8

  acc_lim_x: 1.0
  acc_lim_theta: 2.0
  acc_lim_y: 0.0

# Goal Tolerance Parametes
  yaw_goal_tolerance: 0.3
  xy_goal_tolerance: 0.15
  # latch_xy_goal_tolerance: false

# Forward Simulation Parameters
  sim_time: 4.0
  vx_samples: 20
  vy_samples: 0
  vtheta_samples: 40

# Trajectory Scoring Parameters
  path_distance_bias: 32.0
  goal_distance_bias: 24.0
  occdist_scale: 0.1
  forward_point_distance: 0.325
  stop_time_buffer: 0.2
  scaling_speed: 0.25
  max_scaling_factor: 0.2

# Oscillation Prevention Parameters
  oscillation_reset_dist: 0.05

# Debugging
  publish_traj_pc : true
  publish_cost_grid_pc: true
  global_frame_id: odom

global_costmap_params.yaml：更改全局和base_link的框架名稱，其他參數進行適當調節。

global_costmap:
   global_frame: /map
   robot_base_frame: /footprint
   update_frequency: 2.0
   publish_frequency: 0.5
   static_map: true
   rolling_window: false
   transform_tolerance: 0.5
   plugins:
     - {name: static_layer,            type: "costmap_2d::StaticLayer"}
     - {name: voxel_layer,             type: "costmap_2d::VoxelLayer"}
     - {name: inflation_layer,         type: "costmap_2d::InflationLayer"}

global_planner_params.yaml：對參數進行適當調節。

GlobalPlanner:                                  # Also see: http://wiki.ros.org/global_planner
  old_navfn_behavior: false                     # Exactly mirror behavior of navfn, use defaults for other boolean parameters, default false
  use_quadratic: true                           # Use the quadratic approximation of the potential. Otherwise, use a simpler calculation, default true
  use_dijkstra: false                            # Use dijkstra's algorithm. Otherwise, A*, default true
  use_grid_path: false                          # Create a path that follows the grid boundaries. Otherwise, use a gradient descent method, default false
  
  allow_unknown: true                           # Allow planner to plan through unknown space, default true
                                                #Needs to have track_unknown_space: true in the obstacle / voxel layer (in costmap_commons_param) to work
  planner_window_x: 0.0                         # default 0.0
  planner_window_y: 0.0                         # default 0.0
  default_tolerance: 0.0                        # If goal in obstacle, plan to the closest point in radius default_tolerance, default 0.0
  
  publish_scale: 100                            # Scale by which the published potential gets multiplied, default 100
  planner_costmap_publish_frequency: 0.0        # default 0.0
  
  lethal_cost: 253                              # default 253
  neutral_cost: 50                              # default 50
  cost_factor: 3.0                              # Factor to multiply each cost from costmap by, default 3.0
  publish_potential: true                       # Publish Potential Costmap (this is not like the navfn pointcloud2 potential), default true

local_costmap_params.yaml：更改全局和base_link的框架名稱，其他參數進行適當調節。

local_costmap:
   global_frame: /map
   robot_base_frame: /footprint
   update_frequency: 5.0
   publish_frequency: 2.0
   static_map: false
   rolling_window: true
   width: 4.0
   height: 4.0
   resolution: 0.05
   origin_x: 5.0
   origin_y: 0
   transform_tolerance: 0.5
   plugins:
    - {name: voxel_layer,      type: "costmap_2d::VoxelLayer"}
    - {name: inflation_layer,     type: "costmap_2d::InflationLayer"}

move_base_params.yaml：對參數進行適當調節。

# Move base node parameters. For full documentation of the parameters in this file, please see
#
#  http://www.ros.org/wiki/move_base
#
shutdown_costmaps: false

controller_frequency: 5.0
controller_patience: 3.0


planner_frequency: 1.0
planner_patience: 5.0

oscillation_timeout: 10.0
oscillation_distance: 0.2

# local planner - default is trajectory rollout
base_local_planner: "dwa_local_planner/DWAPlannerROS"

base_global_planner: "navfn/NavfnROS" #alternatives: global_planner/GlobalPlanner, carrot_planner/CarrotPlanner

navfn_global_planner_params.yaml：對參數進行適當調節。


NavfnROS:
  visualize_potential: false    #Publish potential for rviz as pointcloud2, not really helpful, default false
  allow_unknown: true          #Specifies whether or not to allow navfn to create plans that traverse unknown space, default true
                                #Needs to have track_unknown_space: true in the obstacle / voxel layer (in costmap_commons_param) to work
  planner_window_x: 0.0         #Specifies the x size of an optional window to restrict the planner to, default 0.0
  planner_window_y: 0.0         #Specifies the y size of an optional window to restrict the planner to, default 0.0
  
  default_tolerance: 0.0        #If the goal is in an obstacle, the planer will plan to the nearest point in the radius of default_tolerance, default 0.0
                                #The area is always searched, so could be slow for big values

主要是在costmap_common_params.yaml中配置傳感器發布的話題。以及在其他文件中配置map、base_link的框架名。
在gmapping中配置move_base的效果就是，可以進行路徑規划，使用2D Nav goal指定所要到達的目標。

AMCL的配置：

amcl 的配置文件：更改激光雷達話題名，和odom、base_link和map的框架名。對其他參數進行適當調節。

<launch>
  <arg name="use_map_topic"   default="true"/>
  <arg name="scan_topic"      default="/mycar/laser/scan"/> 
  <arg name="initial_pose_x"  default="0.0"/>
  <arg name="initial_pose_y"  default="0.0"/>
  <arg name="initial_pose_a"  default="0.0"/>
  <arg name="odom_frame_id"   default="odom"/>
  <arg name="base_frame_id"   default="footprint"/>
  <arg name="global_frame_id" default="map"/>

  <node pkg="amcl" type="amcl" name="amcl">
    <param name="use_map_topic"             value="$(arg use_map_topic)"/>
    <!-- Publish scans from best pose at a max of 10 Hz -->
    <param name="odom_model_type"           value="diff"/>
    <param name="odom_alpha5"               value="0.1"/>
    <param name="gui_publish_rate"          value="10.0"/>
    <param name="laser_max_beams"             value="810"/>
    <param name="laser_max_range"           value="-1"/>
    <param name="min_particles"             value="500"/>
    <param name="max_particles"             value="5000"/>
    <param name="kld_err"                   value="0.05"/>
    <param name="kld_z"                     value="0.99"/>
    <param name="odom_alpha1"               value="0.2"/>
    <param name="odom_alpha2"               value="0.2"/>
    <!-- translation std dev, m -->
    <param name="odom_alpha3"               value="0.2"/>
    <param name="odom_alpha4"               value="0.2"/>
    <param name="laser_z_hit"               value="0.5"/>
    <param name="laser_z_short"             value="0.05"/>
    <param name="laser_z_max"               value="0.05"/>
    <param name="laser_z_rand"              value="0.5"/>
    <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_likelihood_max_dist" value="2.0"/>
    <param name="update_min_d"              value="0.1"/>
    <param name="update_min_a"              value="0.2"/>
    <param name="odom_frame_id"             value="$(arg odom_frame_id)"/> 
    <param name="base_frame_id"             value="$(arg base_frame_id)"/> 
    <param name="global_frame_id"           value="$(arg global_frame_id)"/>
    <param name="resample_interval"         value="1"/>
    <!-- Increase tolerance because the computer can get quite busy -->
    <param name="transform_tolerance"       value="1.0"/>
    <param name="recovery_alpha_slow"       value="0.0"/>
    <param name="recovery_alpha_fast"       value="0.0"/>
    <param name="initial_pose_x"            value="$(arg initial_pose_x)"/>
    <param name="initial_pose_y"            value="$(arg initial_pose_y)"/>
    <param name="initial_pose_a"            value="$(arg initial_pose_a)"/>
    <remap from="scan"                      to="$(arg scan_topic)"/>
  </node>
</launch>

launch啟動文件：引入Map server已經建立的地圖，指定初始位置並啟動move_base。

<launch>
  <!-- Map server--> 
  <arg name="map_file" default="$(find slam_sim_demo)/maps/Software_Museum.yaml"/>
  <node name="map_server" pkg="map_server" type="map_server" args="$(arg map_file)" />

  <!-- Localization -->
  <arg name="initial_pose_x" default="0.0"/>
  <arg name="initial_pose_y" default="0.0"/>
  <arg name="initial_pose_a" default="0.0"/>
  <arg name="custom_amcl_launch_file" default="$(find mycar_navigation)/launch/include/robot_amcl.launch.xml"/> 

  <include file="$(arg custom_amcl_launch_file)">
    <arg name="initial_pose_x" value="$(arg initial_pose_x)"/>
    <arg name="initial_pose_y" value="$(arg initial_pose_y)"/>
    <arg name="initial_pose_a" value="$(arg initial_pose_a)"/>
  </include>

  <!-- Move base -->
 <include file="$(find mycar_navigation)/launch/include/move_base.launch.xml"/>

</launch>

地圖的格式是 .pgm 格式，配置文件為 .yaml格式：

image: Software_Museum.pgm
resolution: 0.050000
origin: [-25.000000, -25.000000, 0.000000]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.196

AMCL的運行效果：

3、利用rosbag記錄地圖

在啟動Gazebo和Rviz以及相關的建圖程序后，執行以下命令激勵激光雷達（更改為自己發布的激光雷達話題）和tf話題發布的所有消息：
```
$ rosbag record -o data.bag  /tf /mycar/laser/scan /tf_static
```
使用rosbag play回放數據，--clock 表示時間信息：
```
$ rosbag play --clock data_2018-08-10-18-27-20.bag
```
利用回放的數據建圖（這里使用gmapping時，一定要將激光雷達的話題和底盤和tf配置為自己設置的）：
```
$ rosparam set use_sim_time true
$ roslaunch mycar_slam gmapping_demo.launch
```
保存建好的地圖：
```
$ rosrun map_server map_saver
```

發布地圖話題：

$ rosrun map_server map_server map.yaml

然后就可以在Rviz中訂閱/map話題了。

參考：中科院軟件所-機器人操作系統入門（ROS入門教程）：https://www.bilibili.com/video/BV1mJ411R7Ni?p=44

iwehdio的博客園：https://www.cnblogs.com/iwehdio/

免責聲明！

本站轉載的文章為個人學習借鑒使用，本站對版權不負任何法律責任。如果侵犯了您的隱私權益，請聯系本站郵箱yoyou2525@163.com刪除。

猜您在找 ROS入門（二） Solideworks生成URDF 並導入rviz ROS RVIZ顯示點雲地圖的二維投影 0321-ROS-帶地圖導航 ROS 八叉樹地圖構建 - 安裝 octomap 和 octomap_server 建圖包！ ROS 八叉樹地圖構建 - 使用 octomap_server 建圖過程總結！ ROS學習筆記（5）——建圖學習 SLAM+語音機器人DIY系列：（六）SLAM建圖與自主避障導航——3.ros-navigation機器人自主避障導航 ros rviz 啟動指定的rviz 文件【ROS學習之rviz】在rviz中顯示圖片 RViz 實時觀測機器人建立導航2D封閉空間地圖過程 (SLAM) ----27