Ros noetic Move_base 监听Move状态 实战使用教程

前言：

        承接上一篇文章，在上一文中我们了解到move_base有几种监听的状态，我一文章中我将开源全部监听代码，本文将从0开始建立监听包，并覆上全部的工程代码，和仿真实操结果。

        本文，还将解决当临时障碍物与机身相交时，机器人回人为自己被“卡住”，局部规划器规划的速度为0，机器人停止运动；即使更换新的任何目标点，机器人均停止不动；move_base假死状态，在经过恢复行为后无果，不知道新的前进途径的情况。用于恢复状态机。

建立监听节点包：

1) 创建包

cd catkin_ws/src 
catkin_create_pkg listen_move_base roscpp rospy std_msgs

 可见已经生成了节点

2）创建cpp文件：

roscd listen_move_base
cd src 
touch listen_move_base.cpp

3） 输入：

#include "ros/ros.h"
#include <move_base_msgs/MoveBaseAction.h>
#include <move_base_msgs/MoveBaseGoal.h>
 
  //获取到达目标的状态
void status_callback(const move_base_msgs::MoveBaseActionResult& msg)
{	
  if(msg.status.status == 0)
	{
		std::cout<<"目标尚未由操作服务器处理，还没有触发"<<std::endl;
	}

	if(msg.status.status == 1)
	{
		std::cout<<"动作服务器当前正在处理目标，正在前进"<<std::endl;
	}

	if(msg.status.status == 2)
	{
		std::cout<<"目标在开始执行后收到一个取消请求，并已完成执行"<<std::endl;
	}

	if(msg.status.status == 3)
	{
		std::cout<<"动作服务器成功实现目标（终端状态）"<<std::endl;
	}

	if(msg.status.status == 4)
	{
		std::cout<<"由于##到某种故障（终端状态）"<<std::endl;
	}

	if(msg.status.status == 5)
	{
		std::cout<<"目标未经处理就被动作服务器拒绝，因为目标无法实现或无效（终端状态）"<<std::endl;
	}

  if(msg.status.status == 6)
	{
		std::cout<<"目标在开始执行后收到一个取消请求，尚未完成执行"<<std::endl;
	}

  if(msg.status.status == 7)
	{
		std::cout<<"目标在开始执行之前收到一个取消请求，但操作服务器尚未确认目标已取消"<<std::endl;
	}

  if(msg.status.status == 8)
	{
		std::cout<<"目标在开始执行之前收到一个取消请求，已成功取消（终端状态）"<<std::endl;
	}

  if(msg.status.status == 9)
	{
		std::cout<<"动作客户端可以确定目标已丢失。这不应该由操作服务器通过有线发送"<<std::endl;
	}

}
 
int main(int argc, char **argv)
{
    ros::init(argc, argv, "get_movebase_status");
    ros::NodeHandle n;
   
    // 订阅话题
	ros::Subscriber goal_sub =n.subscribe("move_base/result", 10, status_callback);
	
    ros::spin(); 
    return 0;
}

4）修改CMakeList.txt

cmake_minimum_required(VERSION 3.0.2)
project(listen_move_base)

## 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
  message_generation
)

## 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 exec_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 exec_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 exec_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 your 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 listen_move_base
   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} )

add_executable(${PROJECT_NAME}_node src/listen_move_base.cpp)

add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

target_link_libraries(${PROJECT_NAME}_node
  ${catkin_LIBRARIES}
)

## Declare a C++ library
# add_library(${PROJECT_NAME}
#   src/${PROJECT_NAME}/listen_move_base.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/listen_move_base_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
# catkin_install_python(PROGRAMS
#   scripts/my_python_script
#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )

## Mark executables for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
# install(TARGETS ${PROJECT_NAME}_node
#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )

## Mark libraries for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
# install(TARGETS ${PROJECT_NAME}
#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
#   RUNTIME DESTINATION ${CATKIN_GLOBAL_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_listen_move_base.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)

5) 主要修改地方

Build 加入cpp 文件路径和依赖

6) 打开编译依赖文件

7) 顺利通过

8) 编写launch文件启动节点

参考这里考虑到有node的节点，因为c++中区域不明显

listen_move_base_node

<?xml version="1.0"?>

<launch>
    <node pkg="listen_move_base" type="listen_move_base_node" name="get_movebase_status" output="screen">
    </node>
</launch>

9) 启动

10）启动真实的仿真界面

启动Rviz

启动节点

roslaunch listen_move_base listen_move_base.launch 

由此可见监听成功，因为action的整个工作状态是异步通信，必须要等到这个过程完成

就像A和B在丢箱子，必须要等到B拿到箱子后，才会给A讲，我拿到箱子了

至于A怎么反应是A的事情

11）关于收到箱子后的B反馈给A ，A作何反映这一点就比较有意思了，如下：

        这里是A可以执行的一些反馈，从而修正了Move_base的导航假死状态

        当临时障碍物与机身相交时，机器人回人为自己被“卡住”，局部规划器规划的速度为0，机器人停止运动；即使更换新的任何目标点，机器人均停止不动；

12）源码解读

具体步骤：①触发条件的判断

当接收到目标点Goal后，move_base的执行状态会通过MoveBaseActionResult中的status反馈([原创]actionlib中ActionServer和ActionClient的状态机)，

其中当执行结果为失败时一般反馈aborted(即status= 4)，

造成aborted的原因有很多，其中以全局规划失败

局部规划失败和震荡失败三种情况为主（关于三种情况的介绍和震荡的定义）

以上的这种机器人停止不动会触发震荡失败。

但触发震荡失败检测必须设定oscillation_timeout和oscillation_distance才能开启（oscillation_timeout表示执行修复操作之前，允许的震荡时间是几秒。 值0意味着永不超时）！

move_base.cpp源码中，当出现震荡信号时，status= 4，

且text会被赋值字符串Robot is oscillating. Even after executing recovery behaviors.  

13）解决方案

我们在上诉中导出了状态这里只需要用订阅到的机器人状态即可完成解决

1. Problem

在导航时，遇到一个情况，就是突然很多人出现在机器人附近，它没办法规划轨迹，然后启动move base的terminal直接就crash了，之后的操作就没办法进行了

2. Solution

解决方案有两种：

    事前：在发生crash前处理突发状况
    事后：发生crash后，怎么重新启动move base以及重新发布目标位置

在这里只提供一个事后的处理方案，事前的解决方案比较麻烦，要到算法里头去找，这个就不容易弄了，事后补救的方案还是容易实现。

3.这里我们主打选择事后处理的方式进行处理：

代码解析：
move_base_stamp： 记录当前时间
move_base_stamp_last：记录上一个时间
MoveBaseStatus = 0：记录当前result的结果
move_base_status = true： 用来判断move base是否crash，true是正常，false就是出问题了
relaunch_status = true： 启动一次move base，避免重复启动

move base本身就会一直发布自身状态信息，topic名是move_base/status，信息类型是actionlib_msgs/GoalStatusArray，包含如下信息

    # uint8 PENDING         = 0   # The goal has yet to be processed by the action server
    # uint8 ACTIVE          = 1   # The goal is currently being processed by the action server
    # uint8 PREEMPTED       = 2   # The goal received a cancel request after it started executing
    #                             #   and has since completed its execution (Terminal State)
    # uint8 SUCCEEDED       = 3   # The goal was achieved successfully by the action server (Terminal State)
    # uint8 ABORTED         = 4   # The goal was aborted during execution by the action server due
    #                             #    to some failure (Terminal State)
    # uint8 REJECTED        = 5   # The goal was rejected by the action server without being processed,
    #                             #    because the goal was unattainable or invalid (Terminal State)
    # uint8 PREEMPTING      = 6   # The goal received a cancel request after it started executing
    #                             #    and has not yet completed execution
    # uint8 RECALLING       = 7   # The goal received a cancel request before it started executing,
    #                             #    but the action server has not yet confirmed that the goal is canceled
    # uint8 RECALLED        = 8   # The goal received a cancel request before it started executing
    #                             #    and was successfully cancelled (Terminal State)
    # uint8 LOST            = 9   # An action client can determine that a goal is LOST. This should not be
    #                             #    sent over the wire by an action server

代码解析：
if( move_base_stamp == move_base_stamp_last) move_base_counting++：如果前后两个时间戳是一样的，累计数就+1
else move_base_counting = 0：时间戳不一样就归0
if (move_base_counting>=100)：当到100,如果按10Hz频率运行，就是10s，具体根据节点的频率和需求来算
if (relaunch_status == true) MoveBaseStatus = -1：把result的结果设成-1，即非列表里的数
if (move_base_status==false)：这时就是出现crash的情况了，开始事后补救
system("gnome-terminal -e 'bash -c \"roslaunch /opt/ros/melodic/share/move_base/move_base.launch; exec bash\"'")：这句很重要！是重新启动move base的指令，最重要的一点是，把move_base.launch放到/opt/ros/melodic/share/move_base路径下，放在/home下启动不了的，当然，如果有大神知道在/home下启动方法，还望告知，感谢
if (relaunch_status == true)：启动一次后，就不启动了，不然就一直重复启动
BasegoalPub.publish(base_goal)：下面就是重新发布goal的位置，根据实际情况调整
if (MoveBaseStatus != -1)：这一步也很重要，就是说只有当result有变化才跳出现在的crash事后补救状态，不然就一直发布goal，因为有时候发布一次goal会没响应，要多发几次，这个跟topic这种通信方式有关，如果换成action或service就不会有这种Miss的问题

4.源码：

//(1) 监听move base状态
#include <move_base_msgs/MoveBaseAction.h>
#include <move_base_msgs/MoveBaseGoal.h> 
#include <actionlib_msgs/GoalStatusArray.h>
 
  ros::Subscriber goal_sub =node.subscribe("move_base/status", 10, status_callback); //监听move base 状态
  ros::Subscriber BaseStatusSub = node.subscribe("/move_base/result",10,  BaseStatusSubCallback); //监听move base运行的结果
  BasegoalPub = node.advertise<geometry_msgs::PoseStamped>("/move_base_simple/goal", 1);//发布目标位置
 
 
ros::Time move_base_stamp;          
ros::Time move_base_stamp_last; 
int MoveBaseStatus = 0;
bool move_base_status = true;
bool relaunch_status = true;
int move_status = 0;
 
void status_callback(const actionlib_msgs::GoalStatusArray& msg)
{	
  move_base_stamp = msg.header.stamp;
  if(!msg.status_list.empty())
  	move_status = msg.status_list.back().status;
 
  // std::cout<<"move base stamp:"<< msg.header.stamp <<std::endl;
}
 
void BaseStatusSubCallback(const move_base_msgs::MoveBaseActionResult& msg)
{
  MoveBaseStatus=msg.status.status;
}
 
    if( move_base_stamp == move_base_stamp_last) move_base_counting++;
    else move_base_counting = 0;
 
    if (move_base_counting>=100)
    {
      move_base_status = false;
      move_base_counting = 0;
      if (relaunch_status == true) MoveBaseStatus = -1;
    }
    move_base_stamp_last = move_base_stamp;
 
	if (move_base_status==false)
    {
      if (relaunch_status == true)
      {
        ROS_INFO("Move base got killed!!!!!!!!!           Try to relaunch move_base");
        system("gnome-terminal -e 'bash -c \"roslaunch /opt/ros/melodic/share/move_base/move_base.launch; exec bash\"'");
        ros::Duration(5.0).sleep();
        relaunch_status = false;
      }
			
      base_goal.header.seq = 1;
      base_goal.header.stamp =ros::Time::now();
      base_goal.header.frame_id = "map";
      base_goal.pose.position.x = 0.0;
      base_goal.pose.position.y = 0.0;
      base_goal.pose.position.z = 0.0;
      base_goal.pose.orientation.x = 0.0;
      base_goal.pose.orientation.y = 0.0;
      base_goal.pose.orientation.z = 0.0;
      base_goal.pose.orientation.w = 1.0;
 
      BasegoalPub.publish(base_goal);
      ros::Duration(2.0).sleep();
      ROS_INFO("Publish move base goal!");
      if (MoveBaseStatus != -1)//这一步也很重要，就是说只有当result有变化才跳出现在的crash事后补救状态，不然就一直发布goal，因为有时候发布一次goal会没响应，要多发几次，这个跟topic这种通信方式有关，如果换成action或service就不会有这种Miss的问题
      {
        ROS_INFO("Recover success!");
        move_base_status = true;
        relaunch_status = true;
      }
    }