自定义ROS layer的调用日志分析

[ INFO] [1557746278.927167199]: Using plugin "gridlayer"

[ INFO] [1557746278.933583197]: pluglib: GridLayer:  matchSize

         size=0.000000 0.000000. pos=0.000000 0.000000. resolution:0.000000

[ INFO] [1557746279.073044789]: pluglib: GridLayer:  reconfigureCB

         enabled_=1

[ INFO] [1557746279.086381217]: pluglib: GridLayer:  onInitialize

[ INFO] [1557746279.299917695]: pluglib: GridLayer:  matchSize

         size=0.000000 0.000000. pos=0.050000 3.000000. resolution:0.000000

[ INFO] [1557746279.302124238]: pluglib: GridLayer:  updateBounds

         pose=0.066218 -0.020099 -0.007818, min=1.066187 -0.027917 max=1.066187 -0.027917

[ INFO] [1557746279.303423143]: pluglib: GridLayer:  updateCosts

         rect=(min)0 21 -> (max)1 22

[ INFO] [1557746280.974408845]: pluglib: GridLayer:  updateBounds
[ INFO] [1557746279.303423143]: pluglib: GridLayer:  updateCosts

[ INFO] [1557746281.302192779]: pluglib: GridLayer:  updateBounds
[ INFO] [1557746279.303423143]: pluglib: GridLayer:  updateCosts

[ INFO] [1557746281.635429436]: pluglib: GridLayer:  updateBounds
[ INFO] [1557746279.303423143]: pluglib: GridLayer:  updateCosts

…

rviz修改初始位置后. Pose会变成新的位置.  Setting pose: 1.870 8.180 0.037 [frame=map]

[ INFO] [1557749408.408905702]: pluglib: Grid  Now:  updateBounds. pose=1.889547 8.191123 0.034500, min=2.888952 8.225616 max=2.888952 8.225616

[ INFO] [1557749408.408993152]: pluglib: GridLayer:  updateCosts enabled_=1

[ INFO] [1557749408.409073535]: pluglib: GridLayer:  updateCosts rect=164 57 -> 165 58

updateBounds 是一直调用的, 调用频率参数是. 调用后如果对*min/max_x/y没有进行改写, 则不调用updateCosts, 否则会调用 updateCosts.

只要找对对应关系即可.

updateBounds. pose=0.074384 -0.026727 -0.007837, min=1.074353 -0.034564 max=1.074353 -0.034564  

è master_grid.setCost(21, 0, value);   rect=0 21 -> 1 22   (1.074353/0.05 = 21    -0.034564/0.05=0) 

Setting pose: 1.580 8.430 0.045 [frame=map]

Grid  Now:  updateBounds. pose=1.604650 8.445980 0.038716, min=2.603901 8.484687 max=2.603901 8.484687

è master_grid.setCost(52, 169,val)    rect=169 52 -> 170 53    (52 = 2.603901/0.05  8.484687/0.05=169 )

一下为源代码

#include <big_layers/grid_layer.h>
#include <pluginlib/class_list_macros.h>
#include <sys/time.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);

  ROS_INFO("pluglib: GridLayer:  %s", __func__);
}


void GridLayer::matchSize()
{
  Costmap2D* master = layered_costmap_->getCostmap();
  resizeMap(master->getSizeInCellsX(), master->getSizeInCellsY(), master->getResolution(),
            master->getOriginX(), master->getOriginY());
  ROS_INFO("pluglib: GridLayer:  %s.  size=%d %d. pos=%lf %lf. resolution:%lf", __func__, master->getSizeInCellsX(), master->getSizeInCellsY(), master->getOriginX(), master->getOriginY(), master->getResolution());
}


void GridLayer::reconfigureCB(costmap_2d::GenericPluginConfig &config, uint32_t level)
{
  enabled_ = config.enabled;
  ROS_INFO("pluglib: GridLayer:  %s, enabled_=%d", __func__, enabled_);
}

static struct timeval tv = {0};
void GridLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x,
                                           double* min_y, double* max_x, double* max_y)
{
  struct timeval tv_now;
  gettimeofday(&tv_now, NULL);
  ROS_INFO("pluglib: GridLayer:  %s. enabled_ = %d, time=%ld", __func__, enabled_, (tv_now.tv_sec-tv.tv_sec)*1000 + (tv_now.tv_usec-tv.tv_usec)/1000 );
  memcpy(&tv, &tv_now, sizeof(tv));

  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;

  ROS_INFO("pluglib: GridLayer:  %s. pose=%lf %lf %lf, mark=%lf %lf ", __func__, robot_x, robot_y, robot_yaw, mark_x, mark_y);
  if(worldToMap(mark_x, mark_y, mx, my)){
           setCost(mx, my,LETHAL_OBSTACLE);
         ROS_INFO("pluglib: SetCost: %lf, %lf -->  %d, %d", mark_x, mark_y, mx, my);
  }

  *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);

  ROS_INFO("pluglib: Grid  Now:  %s. pose=%lf %lf %lf, min=%lf %lf max=%lf %lf", __func__, robot_x, robot_y, robot_yaw, *min_x, *min_y, *max_x, *max_y);
}

void GridLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i,
                                          int max_j)
{
  ROS_INFO("pluglib: GridLayer:  %s enabled_=%d", __func__, enabled_);
  if (!enabled_)
    return;


  ROS_INFO("pluglib: GridLayer:  %s rect=%d %d -> %d %d", __func__, min_j, min_i, max_j, max_i);
  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]);
      ROS_INFO(" SetCost: %d -->  %d, %d", costmap_[index], i, j);
    }
  }
}

} // end namespace