ROS costmap_2d 插件问题

超声波/自定义图层的插件.

<?xml version="1.0"?>
<package format="2">
  <name>big_layers</name>
  <version>0.0.0</version>
  <description>The big_layers package</description>
  <maintainer email="[e-mail]">ray</maintainer>
  <license>TODO</license>

  <buildtool_depend>catkin</buildtool_depend>
  <build_depend>costmap_2d</build_depend>
  <build_depend>dynamic_reconfigure</build_depend>
  <build_depend>roscpp</build_depend>
  <build_depend>sensor_msgs</build_depend>
  <build_depend>tf</build_depend>
  <build_depend>rospy</build_depend>
  <build_depend>std_msgs</build_depend>
  <build_export_depend>costmap_2d</build_export_depend>
  <build_export_depend>dynamic_reconfigure</build_export_depend>
  <build_export_depend>roscpp</build_export_depend>
  <build_export_depend>sensor_msgs</build_export_depend>
  <build_export_depend>tf</build_export_depend>
  <build_export_depend>rospy</build_export_depend>
  <build_export_depend>std_msgs</build_export_depend>
  <exec_depend>costmap_2d</exec_depend>
  <exec_depend>dynamic_reconfigure</exec_depend>
  <exec_depend>roscpp</exec_depend>
  <exec_depend>sensor_msgs</exec_depend>
  <exec_depend>tf</exec_depend>
  <exec_depend>rospy</exec_depend>
  <exec_depend>std_msgs</exec_depend>
  <depend>big_msgs</depend>

  <export>
    <!-- Other tools can request additional information be placed here -->
    <costmap_2d plugin="${prefix}/costmap_plugins.xml" />
  </export>
</package>

cmake_minimum_required(VERSION 2.8.3)
project(big_layers)

find_package(catkin REQUIRED COMPONENTS
  costmap_2d
  dynamic_reconfigure
  roscpp
  sensor_msgs
  tf
  big_msgs
  rospy
  std_msgs
)

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

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

###################################
## catkin specific configuration ##
###################################
catkin_package(
)

###########
## Build ##
###########
include_directories(
  include
  ${catkin_INCLUDE_DIRS}
)

## Declare a C++ library
add_library(simple_layer
  src/simple_layer.cpp
)
add_library(grid_layer
  src/grid_layer.cpp
)
add_dependencies(simple_layer ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
add_dependencies(grid_layer ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(simple_layer ${catkin_LIBRARIES})
target_link_libraries(grid_layer ${catkin_LIBRARIES})
install(TARGETS simple_layer grid_layer
        LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
        )

add_executable(sonic_layer_node src/sonic_layers.cpp)
add_dependencies(sonic_layer_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(sonic_layer_node
   ${catkin_LIBRARIES}
 )
#############
## Install ##
#############
install(TARGETS sonic_layer_node
   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )
install(FILES
   costmap_plugins.xml
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)

超声波源码.

/* sonic_layers.cpp, 从前后两个超声波获得信息,并通过topic发布到 local_costmap中去 */
#include "ros/ros.h"
#include "sensor_msgs/Range.h"
#include "std_msgs/Header.h"

#include "big_msgs/ChassisSensorState.h"

#include <time.h>
#include <sstream>
#include <tf/transform_broadcaster.h>

typedef struct {
  ros::Subscriber sub_sensors; //碰撞/防跌落信息.
  ros::Publisher sonar0_pub;
  tf::TransformBroadcaster broadcaster;
} G_CTX_T;

boost::shared_ptr<G_CTX_T> gCtx;

void cbSensors(const big_msgs::ChassisSensorState sensor)
{
      //std::cout << "SensorMessage: " << msg << std::endl ;
    sensor_msgs::Range msg;
    std_msgs::Header header;
    header.stamp = ros::Time::now();
    header.frame_id = "sonar0";
    msg.header = header;
    msg.field_of_view = 1;
    msg.min_range = 0;
    msg.max_range = 5;
    msg.range = sensor.sonar / 1000; //毫米 -> 米
    gCtx->broadcaster.sendTransform(
      tf::StampedTransform(
        tf::Transform(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0.1, 0.0, 0.2)),
        ros::Time::now(),
        "base_link",
        "sonar0"
      )
    );

    gCtx->sonar0_pub.publish(msg);
    ROS_INFO("Sonic:%lf", msg.range);
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "UltraSonicLayers");
  // Exit if no ROS
  if (!ros::isInitialized()) {
    return 0;
  }

  gCtx.reset(new G_CTX_T());

  ros::NodeHandle n;
  gCtx->sonar0_pub = n.advertise<sensor_msgs::Range>("/sonar0", 10);
  gCtx->sub_sensors = n.subscribe("/sensor_state", 1, cbSensors); // //碰撞/防跌落信息.

  ros::Rate loop_rate(100);
  while (ros::ok())
  {
    ros::spinOnce();
    loop_rate.sleep();
  }
  return 0;
}

自定义图层代码

#include <big_layers/grid_layer.h>
#include <pluginlib/class_list_macros.h>

PLUGINLIB_EXPORT_CLASS(simple_layer_namespace::GridLayer, costmap_2d::Layer)

using costmap_2d::LETHAL_OBSTACLE;
using costmap_2d::NO_INFORMATION;
using costmap_2d::FREE_SPACE;

namespace simple_layer_namespace
{

unsigned flag = 0;

GridLayer::GridLayer() {}

void GridLayer::onInitialize()
{
  ros::NodeHandle nh("~/" + name_);
  current_ = true;
  default_value_ = NO_INFORMATION;
  matchSize();

  dsrv_ = new dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig>(nh);
  dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig>::CallbackType cb = boost::bind(
      &GridLayer::reconfigureCB, this, _1, _2);
  dsrv_->setCallback(cb);
}


void GridLayer::matchSize()
{
  Costmap2D* master = layered_costmap_->getCostmap();
  resizeMap(master->getSizeInCellsX(), master->getSizeInCellsY(), master->getResolution(),
            master->getOriginX(), master->getOriginY());
}


void GridLayer::reconfigureCB(costmap_2d::GenericPluginConfig &config, uint32_t level)
{
  enabled_ = config.enabled;
}

void GridLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x,
                                           double* min_y, double* max_x, double* max_y)
{
  if (!enabled_)
    return;

  if (flag == 0)
  {
      flag = 1;
  }else
    return;

  double mark_x = robot_x + cos(robot_yaw);
  double mark_y = robot_y + sin(robot_yaw);
  unsigned int mx;
  unsigned int my;
  if(worldToMap(mark_x, mark_y, mx, my)){
           setCost(mx, my,LETHAL_OBSTACLE);
  }

  *min_x = std::min(*min_x, mark_x);
  *min_y = std::min(*min_y, mark_y);
  *max_x = std::max(*max_x, mark_x);
  *max_y = std::max(*max_y, mark_y);
}

void GridLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i,
                                          int max_j)
{
  if (!enabled_)
    return;

  for (int j = min_j; j < max_j; j++)
  {
    for (int i = min_i; i < max_i; i++)
    {
      int index = getIndex(i, j);
      if (costmap_[index] == NO_INFORMATION)
        continue;
      master_grid.setCost(i, j, costmap_[index]);
    }
  }
}

} // end namespace

最最重要的是图层的配置加载问题.

obstacle_range: 3.0        #检测在区域内的障碍, 超过此距离, 默认用SLAM的地图的内容.
raytrace_range: 3.5

#footprint: [[-0.3, -0.3], [-0.3, 0.3], [0.3, 0.3], [0.3, -0.3]]
robot_radius: 0.3           #危险区域的距离.

inflation_radius: 0.5       # Shallow water 浅水区
cost_scaling_factor: 3.0

map_type: costmap
observation_sources: scan
scan: {sensor_frame: base_scan, data_type: LaserScan, topic: /scan, marking: true, clearing: true}

sonar_sensor_layer:
  clear_threshold: 0.46                    #概率低于clear_threshold的单元格被标记为空闲空间
  mark_threshold: 0.98                    #概率高于mark_threshold的细胞被标记为致命障碍
  topics: ["/sonar0", "/sonar1"] # 订阅超神波主题，添加到sonar_layer到 costmap中
  clear_on_max_reading: true                  #是否清楚超过最大范围的数值.
  no_readings_timeout: 0.0

local_costmap:
  global_frame: /odom
  robot_base_frame: /base_footprint

  update_frequency: 8.0
  publish_frequency: 8.0
  transform_tolerance: 0.5

  static_map: false
  rolling_window: true
  width: 3
  height: 3
  resolution: 0.05

  plugins:  #缩进很重要
    - {name: sonar_sensor_layer, type: "range_sensor_layer::RangeSensorLayer"}

global_costmap:
  global_frame: /map
  robot_base_frame: /base_footprint

  update_frequency: 3.0
  publish_frequency: 3.0
  transform_tolerance: 0.5

  static_map: true

  plugins: #缩进很重要
    - {name: gridlayer, type: "simple_layer_namespace::GridLayer"}

big_layers.zip  包括了自定图层以及navigation的param设置.