Karto SLAM 建图与导航 Ubuntu 18.04 Kuboki

ROS部署

请参阅我之前的文章：Ubuntu 18.04 ROS

建立工作空间

mkdir catkin_ws/src
cd catkin_ws/src
catkin_init_workspace
cd ..
catkin_make

Karto SLAM 安装

sudo apt install ros-melodic-slam-karto

Kobuki 配置

sudo apt install ros-melodic-kobuki* ros-melodic-ecl-streams

下载 kobuki_node 和 kobuki_keyop 到catkin_ws/src中，链接
编译

cd catkin_ws
catkin_make

连接kobuki
配置udev别名

rosrun kobuki_ftdi create_udev_rules

检查

ls /dev

如果出现kobuki说明连接成功
启动底盘

source devel/setup.bash
roslaunch kobuki_node minimal.launch

激光雷达配置

rplidar A2

cd catkin_ws/src
git clone https://github.com/Slamtec/rplidar_ros
cd rplidar_ros/scripts
./create_udev_rules.sh

重新插拔雷达
修改启动文件
在rplidar_ros/launch/rplidar.launch中的内添加

<node pkg="tf" type="static_transform_publisher" name="base_to_link" args="0 0 0 0 0 0 base_footprint base_link 100" />
<node pkg="tf" type="static_transform_publisher" name="base_to_gyro" args="0 0 0 0 0 0 base_footprint gyro_link 100" /> 
<node pkg="tf" type="static_transform_publisher" name="base_to_laser" args="0.2 0 0.2 0 0 0  base_footprint laser 100" />

启动雷达

cd catkin_ws
source devel/setup.bash
roslaunch rplidar_ros rplidar.launch

建图

创建control_test功能包

cd catkin_ws/src
catkin_create_pkg control_test  roscpp rospy
cd control_test 
mkdir launch
cd ../../
catkin_make

编写启动文件 karto_slam.launch , 放在contro_test/launch中

<launch>
<node pkg = "slam_karto" type = "slam_karto" name = "slam_karto" output = "screen" >
    <!-- <remap from = "scan" to = "scan"/> -->
    <param name = "odom_frame" value = "odom"/>
    <param name="base_frame" value="base_link"/>
    <!-- <param name="map_frame" value="map"> -->
    <param name = "map_update_interval" value = "25"/>
    <param name = "resolution" value = "0.025"/>
</node>
</launch>

启动karto_slam

roslaunch  control_test karto_slam.launch

启动kobuki键盘控制节点

cd catkin_ws
source devel/setup.bash
roslaunch kobuki_ketop keyop.launch

打开rviz

rviz

添加map和laser话题

控制kobuki移动建立地图

保存地图

cd catkin_ws/src/control_test
mkdir map
cd map
rosrun map_server map_saver -f map

将地图保存在了control_test/map目录下，并命名为map

导航

结束上面所有进程，将kobuki放回建图起始位置。
编写导航launch文件，navigation.launch 并放在control_test/launch中
navigation.launch

<launch>
<include file="$(find rplidar_ros)/launch/rplidar.launch" />                              #启动雷达
<include file="$(find kobuki_node)/launch/minimal.launch" />						#启动底盘
 <arg name="map_file" default="$(find control_test)/map/map.yaml"/>    #地图存放位置
 <node name="map_server_for_test" pkg="map_server" type="map_server" args="$(arg map_file)">
 </node>

 <arg name="use_map_topic" default="false"/>
 <arg name="scan_topic" default="scan"/> 

  <node pkg="amcl" type="amcl" name="amcl" clear_params="true">
    <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="60"/>
    <param name="laser_max_range" value="12.0"/>
    <param name="min_particles" value="500"/>
    <param name="max_particles" value="2000"/>
    <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.25"/>
    <param name="update_min_a" value="0.2"/>
    <param name="odom_frame_id" value="odom"/>
    <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"/>
    <remap from="scan" to="$(arg scan_topic)"/>
</node>

  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
    <rosparam file="$(find control_test)/param/costmap_common_params.yaml" command="load" ns="global_costmap" />
    <rosparam file="$(find control_test)/param/costmap_common_params.yaml" command="load" ns="local_costmap" />
    <rosparam file="$(find control_test)/param/local_costmap_params.yaml" command="load" />
    <rosparam file="$(find control_test)/param/global_costmap_params.yaml" command="load" />
    <rosparam file="$(find control_test)/param/move_base_params.yaml" command="load" />
    <rosparam file="$(find control_test)/param/dwa_local_planner_params.yaml" command="load" />
  </node>
  
  <node name="transsubscribe" pkg="control_test" type="transsubscribe"/>   
  # 接收话题cmd_vel 的消息，然后发给/mobile_base/commands/velocity 起到一个转发的作用
  </launch>

创建transsubscribe.cpp

cd catkin_ws/src/control_test/src
touch transsubscribe.cpp

#include<ros/ros.h>
#include<string>
#include<iostream>
#include<geometry_msgs/Pose.h>
#include<nav_msgs/Odometry.h>
#include<geometry_msgs/Twist.h>
#include<sensor_msgs/Imu.h>
#include<time.h>


ros::Publisher mobile_velocity;

void trans_callback(const geometry_msgs::Twist& msg){
    mobile_velocity.publish(msg);
}

int main(int argc, char **argv){
    
    ros::init(argc,argv,"transsubscribe");
    ros::NodeHandle nh;
    mobile_velocity = nh.advertise<geometry_msgs::Twist>("/mobile_base/commands/velocity",10);
    ros::Subscriber pose_sub = nh.subscribe("cmd_vel" , 10, trans_callback);
    ros::spin();
    return 0;
}

修改control_test的CMakeLists.txt

add_executable(transsubscribe src/transsubscribe.cpp)
target_link_libraries(transsubscribe ${catkin_LIBRARIES})

编译

cd catkin_ws
catkin_make

创建param文件

cd catkin_ws/src/control_test
mkdir param
cd param

touch costmap_common_params.yaml

max_obstacle_height: 0.6  # 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.20  # 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
# 17
#map_type: voxel

obstacle_layer:
  enabled:              true
  max_obstacle_height:  2.0
  min_obstacle_height:  0.0
  #origin_z:             0.0
  #z_resolution:         0.2
  #z_voxels:             2
  #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.0
  raytrace_range: 5.0
  #origin_z: 0.0
  #z_resolution: 0.2
  #z_voxels: 2
  publish_voxel_map: false
  observation_sources:  scan
  scan:
    data_type: LaserScan
    topic: "/scan"
    marking: true
    clearing: true
    expected_update_rate: 0


#cost_scaling_factor and inflation_radius were now moved to the inflation_layer ns
inflation_layer:
  enabled:              true
  cost_scaling_factor:  10.0  # exponential rate at which the obstacle cost drops off (default: 10)
  inflation_radius:     0.15  # max. distance from an obstacle at which costs are incurred for planning paths.
#20
static_layer:
  enabled:              true
  map_topic:            "/map"

touch local_costmap_params.yaml

local_costmap:
   global_frame: odom
   robot_base_frame: base_footprint
   update_frequency: 3.0
   publish_frequency: 1.0
   static_map: false
   rolling_window: true
   width: 2.0
   height: 2.0
   resolution: 0.05
   transform_tolerance: 1
   plugins:
    - {name: obstacle_layer,      type: "costmap_2d::ObstacleLayer"}
    - {name: inflation_layer,     type: "costmap_2d::InflationLayer"}

touch global_costmap_params.yaml

global_costmap:
   global_frame: map
   robot_base_frame: base_footprint
   update_frequency: 1.0
   publish_frequency: 0.5
   static_map: true
   transform_tolerance: 1
   plugins:
     - {name: static_layer,            type: "costmap_2d::StaticLayer"}
     - {name: obstacle_layer,          type: "costmap_2d::VoxelLayer"}
     - {name: inflation_layer,         type: "costmap_2d::InflationLayer"}

touch 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: 3.0
controller_patience: 15.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

touch dwa_local_planner_params.yaml

DWAPlannerROS:

# Robot Configuration Parameters - Kobuki
  max_vel_x: 0.15  # 0.55  机器人在x方向到最大速度，单位为 m/s
  min_vel_x: 0.0   #  机器人在x方向到最小速度，单位为 m/s 

  max_vel_y: 0.0  # diff drive robot
  min_vel_y: 0.0  # diff drive robot

  max_trans_vel: 0.5 # choose slightly less than the base's capability  机器人最大平移速度的绝对值，单位为 m/s 
  min_trans_vel: 0.1  # this is the min trans velocity when there is negligible rotational velocity  机器人最小平移速度的绝对值，单位为 m/s 
  trans_stopped_vel: 0.1

  # Warning!
  #   do not set min_trans_vel to 0.0 otherwise dwa will always think translational velocities
  #   are non-negligible and small in place rotational velocities will be created.

  max_rot_vel: 0.7  # choose slightly less than the base's capability  机器人的最大角速度的绝对值，单位为 rad/s 
  min_rot_vel: 0.5  # this is the min angular velocity when there is negligible translational velocity
  rot_stopped_vel: 0.4
  
  acc_lim_x: 2.0 # maximum is theoretically 2.0, but we    机器人在x方向的加速度极限，单位为 meters/sec^2 
  acc_lim_theta: 3.5  #机器人的角加速度极限，单位为 radians/sec^2  
  acc_lim_y: 0.0      # diff drive robot  # 机器人在y方向的加速度极限，单位为 meters/sec^2 
  # 加速度限制（acc_lim_x，acc_lim_y，acc_lim_th）非常的重要，如果不知道机器人的加速度，
  # 可以尽量的往大的设置，因为如果设置太小了，往往会出现机器人往前跑断断续续的，转弯转过头（看似加速度太大了，实际是加速度太小，以至于机器人想把机器人掰回来而掰不及），从而导致反复的震荡

# Goal Tolerance Parameters
  # yaw_goal_tolerance: 0.15  # 0.05  The tolerance in radians for the controller in yaw/rotation when achieving its goal
  yaw_goal_tolerance: 6.3
  xy_goal_tolerance: 0.15  # 0.10   The tolerance in meters for the controller in the x & y distance when achieving a goal 
  # latch_xy_goal_tolerance: false

# Forward Simulation Parameters
  sim_time: 1.8      # 1.7
  vx_samples: 6       # 3
  vy_samples: 1       # diff drive robot, there is only one sample
  vtheta_samples: 30  # 20

# Trajectory Scoring Parameters
  # path_distance_bias: 64.0      # 32.0   - weighting for how much it should stick to the global path plan 刻画局部路径和全局路径的贴合程度，该权重参数越大说明越贴合，越小说明不用那么贴合
  path_distance_bias: 64.0
  goal_distance_bias: 50      # 24.0   - wighting for how much it should attempt to reach its goal 达到局部目标点的权重参数，也用来控制速度。权重如果设置为0表示要求完全到达目标点
  #这会导致机器人走动缓慢和震荡的现象，因为要求达到目标点的精度太高，所对机器人的控制比较苛刻
  occdist_scale: 0.5            # 0.01   - weighting for how much the controller should avoid obstacles
  forward_point_distance: 0.325 # 0.325  - how far along to place an additional scoring point
  stop_time_buffer: 0.2         # 0.2    - amount of time a robot must stop in before colliding for a valid traj.
  scaling_speed: 0.25           # 0.25   - absolute velocity at which to start scaling the robot's footprint
  max_scaling_factor: 0.2       # 0.2    - how much to scale the robot's footprint when at speed.

# Oscillation Prevention Parameters
  oscillation_reset_dist: 0.05  # 0.05   - how far to travel before resetting oscillation flags

# Debugging
  publish_traj_pc : true
  publish_cost_grid_pc: true
  global_frame_id: odom


# Differential-drive robot configuration - necessary?
#  holonomic_robot: false

启动导航

cd catkin_ws
source devel/setup.bash
roslaunch control_test navigation.launch

打开rviz

rviz