导航功能包入门2

  注意：  

• 再学习本系列教程时，应该已经安装过ROS了并且需要有一些ROS的基本知识

• 本章节需要安装两个功能包
  • 安装amcl：在已有的地图内定位机器人
    • 安装方法1： 在catkin_ws/src下

        $ git clone https://github.com/ros-planning/navigation.git
      

    • 安装方法2

        $ sudo apt-get update
        $ sudo apt-get install ros-melodic-amcl
      

  • 安装gmapping:由激光雷达数据生成地图（或者深度相机数据）
    • 安装方法1： 在catkin_ws/src下

        $ git clone https://github.com/ros-perception/slam_gmapping.git
      

    • 安装方法2：

        $ sudo apt-get update
        $ sudo apt-get install ros-melodic-gmapping
      

webots版本：2020b rev1 ros版本：melodic   在前面几章中分别介绍了在webots中如何创建自己的机器人、添加传感器以及使用手柄或键盘驱动它在仿真环境中移动。在本章中,你会学习到ROS系统最强大的特性之一，它能够让你的机器人自主导航和运动。  

1.使用ROS创建地图

  创建地图本身是一个很麻烦的工作，必须选择正确的工具来简化这项工作。在ROS中就提供了这样的工具能够帮助我们使用激光雷达和机器人的里程信息创建地图。这个工具是map_server地图服务器(http://wiki.ros.org/map_server)。在本示例中,你将会学到如何使用我们在webots中创建的机器人来创建、保存和加载地图。   我们将会使用一个launch文件来简化创建的过程。在webots_demo/launch文件夹下以slam.launch为名称创建一个新文件,然后添加以下代码:  

<launch>
    <!-- 打开webots -->
    <include file="$(find webots_demo)/launch/webots.launch" />
    <arg name="model" />
    <arg name="gui" default="false" />
    <param name="use_gui" value="$(arg gui)" />
    <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
    <node name="robot_broadcaster" pkg="webots_demo" type="robot_broadcaster" />
    <!-- 打开rviz -->
    <node name="rviz" pkg="rviz" type="rviz" />

    <!-- Gmapping建图算法，接收传感器数据建立地图 -->
    <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen" clear_params="true">
    <param name="odom_frame" value="odom" />
    <param name="base_frame" value="base_link" />
    <param name="map_update_interval" value="0.75" />
    <!-- Set maxUrange < actual maximum range of the Laser -->
    <param name="maxRange" value="4.5" />
    <param name="maxUrange" value="3.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="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.157" />
    <param name="temporalUpdate" value="-1.0" />
    <param name="resampleThreshold" value="0.5" />
    <param name="particles" value="50" />

    <param name="xmin" value="-1.0" />
    <param name="ymin" value="-1.0" />
    <param name="xmax" value="1.0" />
    <param name="ymax" value="1.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="/robot/Sick_LMS_291/laser_scan/layer0" />
  </node>
</launch>

有了这个.launch文件，就可以迅速打开webots仿真环境、rviz和gmapping。 可以通过rviz和slam_gmapping算法来实时构建地图。 在控制台中启动这个launch文件:  

$ roslaunch webots_demo slam.launch

可以使用第三章编写的控制程序来控制机器人建立地图：

$ rosrun webots_demo velocity_keyboard

建图过程如下：  
  最终建图效果：  

1.1 使用map_server保存地图

  完成了整个地图的探索，就可以通过map_server功能包将地图保存下来方便下次调用。我们可以通过以下命令来保存地图：  

$ rosrun map_server map_saver -f map

[ INFO] [1610525218.211739274]: Waiting for the map
[ INFO] [1610525218.422685318]: Received a 480 X 544 map @ 0.050 m/pix
[ INFO] [1610525218.422736024]: Writing map occupancy data to map.pgm
[ INFO] [1610525218.428464420]: Writing map occupancy data to map.yaml
[ INFO] [1610525218.428634957]: Done

  这个命令会创建两个文件，map.pgm和map.yaml。第一个文件是以.pem为格式的地图(可输出灰色地图格式)。另一个则是该地图的配置文件。 将这两个文件放至webots_demo/config下，应当先创建该文件夹，下面会用到。  

2.导航配置

2.1 配置全局和局部代价地图

  全局和局部代价地图分别是什么？ 机器人将使用这两种导航算法在地图中移动  

• 全局导航（global）
  • 全局导航用于建立到地图上最终目标或一个远距离目标的路径。
•  局部导航（local）
  • 局部导航用于建立到近距离目标和为了临时躲避障碍物的路径。例如,机器人四周一个4x4m的方形窗户。

  这些导航算法通过使用代价地图来处理地图中的各种信息。全局代价地图用于全局导航,局部代价地图用于局部导航。 代价地图的参数用于配置算法计算行为。它们也有最基本的通用参数,这些参数会保存在共享的文件中。 我们可以在三个最基本的配置文件中设定不同的参数。这三个文件如下:  

• costmap_common_params.yaml
• global_costmap_params.yaml
• local_costmap_params.yaml 你只需要看一下这三个文件的名称,就能大致猜出来它们的用途。
  

  2.1.1 基本参数的配置

  在webots_demo/config下以costmap_common_params.yaml为名称创建一个新文件，并且添加以下代码：

#“obstacle_range”参数决定了引入障碍物到代价地图的传感器读数的最大范围。我们把它设定为2.5米,这意味着机器人只会更新以其底盘为中心半径2.5米内的障碍信息.
obstacle_range: 2.5
#“raytrace_range”参数确定的空白区域内光线追踪的范围。 设置为3.0米意味着机器人将试图根据传感器读数清除其前面3.0米远的空间。
raytrace_range: 3.0
#指定的footprint时,机器人的中心被认为是在(0.0,0.0),顺时针和逆时针规范都支持。 
footprint: [[0.25, 0.25], [0.25, -0.25],[-0.25, -0.25],[-0.25, 0.25]]
#robot_radius: ir_of_robot
#代价地图膨胀半径
inflation_radius: 0.55
#“observation_sources”参数定义了一系列传递空间信息给代价地图的传感器
observation_sources: laser_scan_sensor #point_cloud_sensor

laser_scan_sensor: {sensor_frame: base_laser, data_type: LaserScan, topic: robot/Sick_LMS_291/laser_scan/layer0, marking: true, clearing: true}

#point_cloud_sensor: {sensor_frame: frame_name, data_type: PointCloud, topic: topic_name, marking: true, clearing: true}

2.1.2 全局代价地图的配置

  在webots_demo/config下以global_costmap_params.yaml为名称创建一个新文件，并且添加以下代码：  

global_costmap:
#“global_frame”参数定义了代价地图运行所在的坐标帧。
  global_frame: map
#“robot_base_frame”参数定义了代价地图参考的的机器底盘的坐标帧。  
  robot_base_frame: base_link
#“update_frequency”参数决定了代价地图更新的频率。
  update_frequency: 5.0
  publish_frequency: 5.0
#“static_map”参数决定代价地图是否根据map_server提供的地图初始化。如果你不使用现有的地图，设为false。
  static_map: true

2.1.3 局部代价地图的配置

  在webots_demo/config下以local_costmap_params.yaml为名称创建一个新文件，并且添加以下代码：  

local_costmap:
  global_frame: odom
  robot_base_frame: base_link
  update_frequency: 5.0
  publish_frequency: 5.0
  static_map: true
#将“rolling_window”参数设置为true，意味着随着机器人在现实世界里移动，代价地图会保持以机器人为中心。
  rolling_window: true
  width: 6.0
  height: 6.0
  resolution: 0.05

2.1.4 基本局部规划器配置

  这个基本规划器会产生一个速度命令来移动机器人。 在webots_demo/config下以base_local_planner_params.yaml为名称创建一个新文件，并且添加以下代码：  

TrajectoryPlannerROS:
  #速度限制
  max_vel_x: 0.45
  min_vel_x: 0.1
  max_vel_theta: 1.0
  min_in_place_vel_theta: 0.4
  #加速度限制
  acc_lim_theta: 3.2
  acc_lim_x: 2.5
  acc_lim_y: 2.5
  #完整的机器人
  holonomic_robot: false

2.2 配置启动文件

  配置好所有功能包必须文件后，只需要配置一个launch文件可以方便我们运行。 在webots_demo/launch文件夹下创建一个名为move_base.launch的新文件，并添加以下代码：  

<launch>
  <master auto="start"/>

  <!-- Run the map server -->
  <node name="map_server" pkg="map_server" type="map_server" args="$(find webots_demo)/config/map.yaml "/>

  <!-- amcl定位-->
  <node pkg="amcl" type="amcl" name="amcl" output="screen">
    <remap from="scan" to="robot/Sick_LMS_291/laser_scan/layer0"/>
    <param name="odom_frame_id" value="odom"/>
    <param name="odom_model_type" value="diff-corrected"/>
    <param name="base_frame_id" value="base_link"/>
    <param name="global_frame_id" value="map"/>

    <!--transform tolerance-->
    <param name="transform_tolerance" value="0.9"/>

    <!--overall filter-->
    <param name="min_particles" value="150"/>
    <param name="max_particles" value="400"/>
    <param name="initial_pose_x" value="0.0"/>
    <param name="initial_pose_y" value="0.0"/>
    <param name="updata_min_a" value="0.1"/>
    <param name="update_min_d" value="0.25"/>

    <!--likelihood_field of laser-->
    <param name="laser_min_range" value="1.0"/>
    <param name="laser_max_range" value="25"/>
    <param name="laser_max_beams" value="30"/>
    <param name="laser_z_hit" value="0.95"/>
    <param name="laser_z_rand" value="0.05"/>

    <!--Odometry-->
    <param name="odom_model_type" value="diff"/>
    <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"/>
  </node>

  <!-- move_base 导航 -->>
  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
    <rosparam file="$(find webots_demo)/config/costmap_common_params.yaml" command="load" ns="global_costmap" />
    <rosparam file="$(find webots_demo)/config/costmap_common_params.yaml" command="load" ns="local_costmap" />
    <rosparam file="$(find webots_demo)/config/local_costmap_params.yaml" command="load" />
    <rosparam file="$(find webots_demo)/config/global_costmap_params.yaml" command="load" />
    <rosparam file="$(find webots_demo)/config/base_local_planner_params.yaml" command="load" />
    <remap from="cmd_vel" to="cmd_vel"/>
    <remap from="odom" to="odom"/>
    <remap from="scan" to="robot/Sick_LMS_291/laser_scan/layer0"/>

    <param name="base_global_planner" type="string" value="navfn/NavfnROS" />
    <param name="base_local_planner" value="base_local_planner/TrajectoryPlannerROS"/>
  </node>
</launch>

3.slam.launch改动

  在这我把之前编辑的slam.launch分成了两个  

• slam_no_gmapping.launch(删除gmapping配置)

  <launch>
    <!-- 打开webots -->
    <include file="$(find webots_demo)/launch/webots.launch" />
    <arg name="model" />
    <arg name="gui" default="false" />
    <!-- 载入机器人模型 -->
    <param name="robot_description" textfile="$(find webots_demo)/urdf/demo.urdf" />
    <param name="use_gui" value="$(arg gui)" />
    <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
    <node name="robot_broadcaster" pkg="webots_demo" type="robot_broadcaster" />
    <!-- 打开rviz -->
    <node name="rviz" pkg="rviz" type="rviz"  args="-d $(find webots_demo)/rviz/demo.rviz"/>
  </launch>
  

• slam_with_gmapping.launch(保留gmapping配置,之前编辑的那份改个名字)
  

  4.运行测试

运行webots、rviz、robot_broadcaster

$ roslaunch webots_demo slam_no_gmapping.launch

如图所示：
  运行move_base.launch，加载地图实现导航

$ roslaunch webots_demo move_base.launch

如图所示：

5.编写指定点导航运动程序

1. 点击rviz上的，在地图上设置一个导航点和导航方向。这时/cmd_vel会发布话题 使用rostopic echo /cmd_vel可以看到数据格式。 格式如下： /cmd_vel会返回一个线性速度和角速度，这时，因为我们给webots的是两轮的速度，所以需要使用算法倒推回去。 算法如下（导航推测）： 因为我们知道了线速度(Vk)和角速度(Wk)的值，那我们可以联立方程算出左轮速度(Vl)和右轮速度(Vr)的值。
2. 在webots_demo/src下创建一个demo_2dnav_move.cpp的程序，这个程序的目的是，在rviz中设置目标点后，将move_base发送的速度角度值给webots机器人，让他能移动到指定位置，添加以下程序：

#include <signal.h>
#include <std_msgs/String.h>
#include "ros/ros.h"
#include <geometry_msgs/Twist.h> 
#include <webots_ros/set_float.h>
#include <webots_ros/set_int.h>
#include <webots_ros/Int32Stamped.h>

using namespace std;

#define TIME_STEP 32    //时钟
#define NMOTORS 2       //电机数量
#define MAX_SPEED 2.0   //电机最大速度

ros::NodeHandle *n;

static int controllerCount;
static std::vector<std::string> controllerList; 

ros::ServiceClient timeStepClient;          //时钟通讯客户端
webots_ros::set_int timeStepSrv;            //时钟服务数据

ros::ServiceClient set_velocity_client;
webots_ros::set_float set_velocity_srv;

ros::ServiceClient set_position_client;   
webots_ros::set_float set_position_srv;   

double speeds[NMOTORS]={0.0,0.0};// 2电机速度值 0～10


// 控制位置电机
static const char *motorNames[NMOTORS] ={"left_motor", "right_motor"};

/*******************************************************
* Function name ：updateSpeed
* Description   ：将速度请求以set_float的形式发送给set_velocity_srv
* Parameter     ：无
* Return        ：无
**********************************************************/
void updateSpeed() {   

    for (int i = 0; i < NMOTORS; ++i) {
        // 更新速度
        set_velocity_client = n->serviceClient<webots_ros::set_float>(string("/robot/") + string(motorNames[i]) + string("/set_velocity"));   
        set_velocity_srv.request.value = -speeds[i];
        set_velocity_client.call(set_velocity_srv);
    }
}

/*******************************************************
* Function name ：controllerNameCallback
* Description   ：控制器名回调函数，获取当前ROS存在的机器人控制器
* Parameter     ：
        @name   控制器名
* Return        ：无
**********************************************************/
// catch names of the controllers availables on ROS network
void controllerNameCallback(const std_msgs::String::ConstPtr &name) { 
    controllerCount++; 
    controllerList.push_back(name->data);//将控制器名加入到列表中
    ROS_INFO("Controller #%d: %s.", controllerCount, controllerList.back().c_str());

}

/*******************************************************
* Function name ：quit
* Description   ：退出函数
* Parameter     ：
        @sig   信号
* Return        ：无
**********************************************************/
void quit(int sig) {
    ROS_INFO("User stopped the '/robot' node.");
    timeStepSrv.request.value = 0; 
    timeStepClient.call(timeStepSrv); 
    ros::shutdown();
    exit(0);
}

void cmdvelDataCallback(const geometry_msgs::Twist::ConstPtr &value)
{
    float linear_temp=0, angular_temp=0;//暂存的线速度和角速度,
    float L = 0.6;//两轮之间的距离
    angular_temp = value->angular.z ;//获取/cmd_vel的角速度,rad/s
    linear_temp = value->linear.x ;//获取/cmd_vel的线速度.m/s
    //将转换好的小车速度分量为左右轮速度
    speeds[0]  = 10.0*(2.0*linear_temp - L*angular_temp)/2.0;
    speeds[1]  = 10.0*(2.0*linear_temp + L*angular_temp)/2.0;
    updateSpeed();
    ROS_INFO("left_vel:%lf,  right_vel:%lf", speeds[0], speeds[1]);
}


int main(int argc, char **argv) {

    std::string controllerName;
    // create a node named 'robot' on ROS network
    ros::init(argc, argv, "robot_init", ros::init_options::AnonymousName);
    n = new ros::NodeHandle;

    signal(SIGINT, quit);

    // subscribe to the topic model_name to get the list of availables controllers
    ros::Subscriber nameSub = n->subscribe("model_name", 100, controllerNameCallback);
    while (controllerCount == 0 || controllerCount < nameSub.getNumPublishers()) {
        ros::spinOnce();
    }
    ros::spinOnce();

    timeStepClient = n->serviceClient<webots_ros::set_int>("robot/robot/time_step");
    timeStepSrv.request.value = TIME_STEP;

    // if there is more than one controller available, it let the user choose
    if (controllerCount == 1)
        controllerName = controllerList[0];
    else {
        int wantedController = 0;
        std::cout << "Choose the # of the controller you want to use:\n";
        std::cin >> wantedController;
        if (1 <= wantedController && wantedController <= controllerCount)
        controllerName = controllerList[wantedController - 1];
        else {
        ROS_ERROR("Invalid number for controller choice.");
        return 1;
        }
    }
    ROS_INFO("Using controller: '%s'", controllerName.c_str());
    // leave topic once it is not necessary anymore
    nameSub.shutdown();

    // init motors 
    for (int i = 0; i < NMOTORS; ++i) {
        // position速度控制时设置为缺省值INFINITY   
        // 初始化2个控制位置的电机
        set_position_client = n->serviceClient<webots_ros::set_float>(string("/robot/") + string(motorNames[i]) + string("/set_position"));   
        set_position_srv.request.value = INFINITY;
        if (set_position_client.call(set_position_srv) && set_position_srv.response.success)     
            ROS_INFO("Position set to INFINITY for motor %s.", motorNames[i]);   
        else     
            ROS_ERROR("Failed to call service set_position on motor %s.", motorNames[i]);   
        set_velocity_client = n->serviceClient<webots_ros::set_float>(string("/robot/") + string(motorNames[i]) + string("/set_velocity"));   
        set_velocity_srv.request.value = 0.0;   
        if (set_velocity_client.call(set_velocity_srv) && set_velocity_srv.response.success == 1)     
            ROS_INFO("Velocity set to 0.0 for motor %s.", motorNames[i]);   
        else     
            ROS_ERROR("Failed to call service set_velocity on motor %s.", motorNames[i]); 
    }   

    ros::Subscriber cmdvelSub;
    cmdvelSub = n->subscribe("/cmd_vel",1,cmdvelDataCallback);
    while (cmdvelSub.getNumPublishers() == 0) {}
    while (ros::ok()) {   
        ros::spinOnce();
        if (!timeStepClient.call(timeStepSrv) || !timeStepSrv.response.success)
        {  
            ROS_ERROR("Failed to call service time_step for next step.");     
            break;   
        }   
        ros::spinOnce();
    } 

    timeStepSrv.request.value = 0;
    timeStepClient.call(timeStepSrv);
    ros::shutdown(); 
    return 0;
}

在这里，主要是一个cmdvelDataCallback回调函数。 别忘了在CMakeLists.txt最后加上如下程序：

add_executable(2dnav_move src/2dnav_move.cpp)
add_dependencies(2dnav_move webots_ros_generate_messages_cpp)
target_link_libraries(2dnav_move    ${catkin_LIBRARIES})

在package.xml中加入以下代码，保证 move_base能被编译。

<build_depend>move_base</build_depend>
<build_export_depend>move_base</build_export_depend>
<exec_depend>move_base</exec_depend>

<!--用在rviz中-->>
<exec_depend>robot_state_publisher</exec_depend> 
<exec_depend>rviz</exec_depend>
<exec_depend>joint_state_publisher</exec_depend>

6.最终的运行

1. 编译

   $ cd catkin_ws/
   $ catkin_make
   

2. 接着上面的运行测试的步骤
3. 运行刚刚我们编辑的demo_2dnav_move.cpp

   $ rosrun webots_demo demo_2dnav_move
   

   运行完后可以看到如下场景：

1. 导航功能，设置目标点，运行结果如下：

结语

本文也是基于笔者的学习和使用经验总结的，主观性较强，如果有哪些不对的地方或者不明白的地方，欢迎评论区留言交流~   到这，这个项目的一部分就已经告一段落了 Bye