ROS 学习- 第27日 自定义 recovery behavior plugin

ROS中, 经常出现 recovery behavior start, 在kinetic中默认用的是 rotate_recovery, 即旋转360, 清除costmap的方式.

但是对于我的机器人使用了UWB进行定位, 激光雷达所做的地图被手动改动了很多, 而且加入了 sonar layer之后, 会有很大的噪点障碍. 用rotate_recovery比较难于恢复和定位. 需要改写.

#include <ros/ros.h>
#include <nav_core/recovery_behavior.h>
#include <costmap_2d/costmap_2d_ros.h>
#include <boost/thread.hpp>
#include <dynamic_reconfigure/Reconfigure.h>

namespace custom_recovery_behavior
{
  class CustomRecoveryAction : public nav_core::RecoveryBehavior
  {
    public:
      CustomRecoveryAction();
      ~CustomRecoveryAction();

      /// Initialize the parameters of the behavior
      void initialize (std::string n, tf::TransformListener* tf,
          costmap_2d::Costmap2DROS* global_costmap,
          costmap_2d::Costmap2DROS* local_costmap);

      /// Run the behavior
      void runBehavior();

    private:
      bool initialized_;
      ros::Publisher custom_recovery_reset;    //costmap/plan/pose 重置
  };
}

#include <custom_recovery_behavior.h>
#include <pluginlib/class_list_macros.h>
#include <costmap_2d/obstacle_layer.h>
#include <std_msgs/Empty.h>

PLUGINLIB_EXPORT_CLASS(custom_recovery_behavior::CustomRecoveryAction, nav_core::RecoveryBehavior)

namespace custom_recovery_behavior
{
  CustomRecoveryAction::CustomRecoveryAction(): initialized_(false) {}

  CustomRecoveryAction::~CustomRecoveryAction() {}

  void CustomRecoveryAction::initialize (std::string n, tf::TransformListener* tf,
      costmap_2d::Costmap2DROS* global_costmap,
      costmap_2d::Costmap2DROS* local_costmap)
  {
    //声明topic, 通知外部app, 清除costmap并重新定位.
    ros::NodeHandle nh;
    custom_recovery_reset = nh.advertise<std_msgs::Empty>("custom_recovery_reset", 1);
    initialized_ = true;
    ROS_WARN("Custom recovery behavior initialize.");
  }

  //自定义 RecoveryBehavior 的动作.
  void CustomRecoveryAction::runBehavior()
  {
    if(!initialized_)
    {
      ROS_ERROR("Custom recovery behavior has not been initialized, doing nothing.");
      return;
    }
    ROS_WARN("Custom recovery behavior started.");

    std_msgs::Empty msg;
    custom_recovery_reset.publish(msg);
  }
}

其他的节点程序中,实现清除costmap : 相当于 rosservice call /move_base/clear_costmaps
然后使用uwb的位置更新机器人位置. 
g_rt_ctx->pub_initalpose = nh.advertise("initialpose", 1);

void clear_costmaps(void)
{
    //clear costmaps.
  std_srvs::Empty srv;
    g_rt_ctx->srv_clear_costmap.call(srv);
}

void set_robot_initialpose(double x, double y, double th)
{
  geometry_msgs::PoseWithCovarianceStamped init_pose;
  init_pose.header.frame_id = "map"; //基于Map的点.
  init_pose.header.stamp = ros::Time::now();

  init_pose.pose.pose.position.x = x; //起始位置点
  init_pose.pose.pose.position.y = y;

  tf::Quaternion q;
  //th 调校到 -180 , 180 区间
  th = (th <= 180)? th : (th - 360);
  double arc = (th) * M_PI/180;
  q.setRPY(0, 0, arc);
  init_pose.pose.pose.orientation.x=q[0];
  init_pose.pose.pose.orientation.y=q[1];
  init_pose.pose.pose.orientation.z=q[2];
  init_pose.pose.pose.orientation.w=q[3];

  g_rt_ctx->pub_initalpose.publish(init_pose);
  clear_costmaps();
}

配置的部分比较重要
package.xml

<?xml version="1.0"?>
<package format="2">
  <name>big_navigation</name>
  <version>1.2.0</version>
  <description>
    The big_navigation provides roslaunch scripts for starting the navigation.
  </description>
  <author email="[e-mail]">iibull</author>
  <license>Apache 2.0</license>
  <maintainer email="[e-mail]">iibull</maintainer>

  <buildtool_depend>catkin</buildtool_depend>
  <exec_depend>amcl</exec_depend>
  <exec_depend>map_server</exec_depend>
  <exec_depend>move_base</exec_depend>
  <exec_depend>big_bringup</exec_depend>
  <build_depend>message_generation</build_depend>
  <build_depend>roscpp</build_depend>
  <build_depend>std_msgs</build_depend>
  <build_export_depend>roscpp</build_export_depend>
  <build_export_depend>std_msgs</build_export_depend>
  <build_export_depend>message_generation</build_export_depend>
  <exec_depend>roscpp</exec_depend>
  <exec_depend>std_msgs</exec_depend>
  <depend>big_msgs</depend>
  <depend>sensor_msgs</depend>
  <depend>tf</depend>

  <build_depend>cmake_modules</build_depend>
  <build_depend>pluginlib</build_depend>
  <exec_depend>pluginlib</exec_depend>
  <build_depend>nav_core</build_depend>
  <exec_depend>nav_core</exec_depend>
  <build_depend>costmap_2d</build_depend>
  <exec_depend>costmap_2d</exec_depend>
  <build_depend>geometry_msgs</build_depend>
  <exec_depend>geometry_msgs</exec_depend>

  <export>
    <nav_core plugin="${prefix}/recovery_plugin.xml" />
  </export>
</package>

CMakeLists.txt

################################################################################
# Set minimum required version of cmake, project name and compile options
################################################################################
cmake_minimum_required(VERSION 2.8.3)
project(big_navigation)

################################################################################
# Find catkin packages and libraries for catkin and system dependencies
################################################################################
find_package(catkin REQUIRED COMPONENTS
  message_generation
  roscpp
  std_msgs
  big_msgs
  tf
  sensor_msgs
  cmake_modules
  costmap_2d
  geometry_msgs
  nav_core
  pluginlib
)

################################################################################
# Setup for python modules and scripts
################################################################################

################################################################################
# Declare ROS messages, services and actions
################################################################################

################################################################################
# Declare ROS dynamic reconfigure parameters
################################################################################

################################################################################
# Declare catkin specific configuration to be passed to dependent projects
################################################################################

catkin_package(
  INCLUDE_DIRS include
  LIBRARIES custom_recovery_behavior
  CATKIN_DEPENDS message_generation roscpp std_msgs big_msgs sensor_msgs tf geometry_msgs nav_core pluginlib
#  DEPENDS system_lib
)

################################################################################
# Build
################################################################################
find_package(Eigen3 REQUIRED)
find_package(PCL REQUIRED)
remove_definitions(-DDISABLE_LIBUSB-1.0)
find_package(Boost REQUIRED COMPONENTS thread)

include_directories(
  include
  ${catkin_INCLUDE_DIRS}
  ${catkin_INCLUDE_DIRS}
  ${EIGEN3_INCLUDE_DIRS}
  ${PCL_INCLUDE_DIRS}
)
add_definitions(${EIGEN3_DEFINITIONS})

add_library(custom_recovery_behavior src/custom_recovery_behavior.cpp)
add_dependencies(custom_recovery_behavior ${catkin_EXPORTED_TARGETS})
target_link_libraries(custom_recovery_behavior
    ${Boost_LIBRARIES}
    ${catkin_LIBRARIES}
)


################################################################################
# Install
################################################################################
install(DIRECTORY launch param rviz
  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)

install(TARGETS custom_recovery_behavior
       ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
       LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
)

install(FILES recovery_plugin.xml
    DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)

## Install project namespaced headers
install(DIRECTORY include/
    DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
    FILES_MATCHING PATTERN "*.h"
    #PATTERN ".svn" EXCLUDE
)


################################################################################
# Test
################################################################################

recovery_plugin.xml

<library path="lib/libcustom_recovery_behavior">
  <class name="custom_recovery_behavior/CustomRecoveryAction" type="custom_recovery_behavior::CustomRecoveryAction" base_class_type="nav_core::RecoveryBehavior">
    <description>
      自定义的recovery behavior. 用于清除costmap,限定运行速度.
    </description>
  </class>
</library>

然后在 move_base.launch 中覆盖默认的 behavior.

<launch>
  <arg name="model" default="$(env BIG_MODEL)" doc="Waffle"/>
  <arg name="cmd_vel_topic" default="/cmd_vel" />
  <arg name="odom_topic" default="odom" />
  <arg name="move_forward_only" default="false"/>
  <arg name="PlannerROS" default="DWAPlannerROS" />
  <rosparam param="recovery_behaviors"> [ { name: "custom_recovery_behavior", type: "custom_recovery_behavior/CustomRecoveryAction" } ] </rosparam>

  <!-- move_base -->
  <group if="$(eval arg('PlannerROS') == 'DWAPlannerROS')">
    <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
      <param name="base_local_planner" value="dwa_local_planner/DWAPlannerROS" />
      <rosparam file="$(find big_navigation)/param/costmap_common_params_$(arg model).yaml" command="load" ns="global_costmap" />
      <rosparam file="$(find big_navigation)/param/costmap_common_params_$(arg model).yaml" command="load" ns="local_costmap" />
      <rosparam file="$(find big_navigation)/param/local_costmap_params.yaml" command="load" />
      <rosparam file="$(find big_navigation)/param/global_costmap_params.yaml" command="load" />
      <rosparam file="$(find big_navigation)/param/move_base_params.yaml" command="load" />
      <rosparam file="$(find big_navigation)/param/dwa_local_planner_params_$(arg model).yaml" command="load" />
      <remap from="cmd_vel" to="$(arg cmd_vel_topic)"/>
      <remap from="odom" to="$(arg odom_topic)"/>
      <param name="DWAPlannerROS/min_vel_x" value="0.0" if="$(arg move_forward_only)" />
    </node>
  </group>

  <group if="$(eval arg('PlannerROS') == 'TrajectoryPlannerROS')">
    <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
      <param name="base_local_planner" value="base_local_planner/TrajectoryPlannerROS" />
      <rosparam file="$(find big_navigation)/param/costmap_common_params_$(arg model).yaml" command="load" ns="global_costmap" />
      <rosparam file="$(find big_navigation)/param/costmap_common_params_$(arg model).yaml" command="load" ns="local_costmap" />
      <rosparam file="$(find big_navigation)/param/local_costmap_params.yaml" command="load" />
      <rosparam file="$(find big_navigation)/param/global_costmap_params.yaml" command="load" />
      <rosparam file="$(find big_navigation)/param/move_base_params.yaml" command="load" />
      <rosparam file="$(find big_navigation)/param/base_local_planner_params.yaml" command="load" />
      <remap from="cmd_vel" to="$(arg cmd_vel_topic)"/>
      <remap from="odom" to="$(arg odom_topic)"/>
      <param name="TrajectoryPlannerROS/min_vel_x" value="0.0" if="$(arg move_forward_only)" />
    </node>
  </group>

</launch>

运行出现

 * /move_base/planner_patience: 5.0
 * /move_base/shutdown_costmaps: False
 * /recovery_behaviors: [{'type': 'custom...
 * /robot_description: 
 * /robot_state_publisher/publish_frequency: 50.0

在local_costmap无路可走的时候, 会被调用.