Chinaunix首页 | 论坛 | 博客
  • 博客访问: 5062888
  • 博文数量: 1200
  • 博客积分: 12961
  • 博客等级: 上将
  • 技术积分: 14456
  • 用 户 组: 普通用户
  • 注册时间: 2009-01-09 11:25
  • 认证徽章:
个人简介

偷得浮生半桶水(半日闲), 好记性不如抄下来(烂笔头). 信息爆炸的时代, 学习是一项持续的工作.

文章分类

全部博文(1200)

文章存档

2019年(177)

2018年(81)

2017年(80)

2016年(70)

2015年(52)

2014年(41)

2013年(51)

2012年(85)

2011年(45)

2010年(231)

2009年(287)

分类: 其他平台

2019-05-13 20:36:41

[ 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 )

一下为源代码

点击(此处)折叠或打开

  1. #include <big_layers/grid_layer.h>
  2. #include <pluginlib/class_list_macros.h>
  3. #include <sys/time.h>
  4.  
  5. PLUGINLIB_EXPORT_CLASS(simple_layer_namespace::GridLayer, costmap_2d::Layer)
  6.  
  7. using costmap_2d::LETHAL_OBSTACLE;
  8. using costmap_2d::NO_INFORMATION;
  9. using costmap_2d::FREE_SPACE;
  10.  
  11. namespace simple_layer_namespace
  12. {
  13.  
  14. unsigned flag = 0;
  15.  
  16. GridLayer::GridLayer() {}
  17.  
  18. void GridLayer::onInitialize()
  19. {
  20.   ros::NodeHandle nh("~/" + name_);
  21.   current_ = true;
  22.   default_value_ = NO_INFORMATION;
  23.   matchSize();
  24.  
  25.   dsrv_ = new dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig>(nh);
  26.   dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig>::CallbackType cb = boost::bind(
  27.       &GridLayer::reconfigureCB, this, _1, _2);
  28.   dsrv_->setCallback(cb);

  29.   ROS_INFO("pluglib: GridLayer: %s", __func__);
  30. }
  31.  
  32.  
  33. void GridLayer::matchSize()
  34. {
  35.   Costmap2D* master = layered_costmap_->getCostmap();
  36.   resizeMap(master->getSizeInCellsX(), master->getSizeInCellsY(), master->getResolution(),
  37.             master->getOriginX(), master->getOriginY());
  38.   ROS_INFO("pluglib: GridLayer: %s. size=%d %d. pos=%lf %lf. resolution:%lf", __func__, master->getSizeInCellsX(), master->getSizeInCellsY(), master->getOriginX(), master->getOriginY(), master->getResolution());
  39. }
  40.  
  41.  
  42. void GridLayer::reconfigureCB(costmap_2d::GenericPluginConfig &config, uint32_t level)
  43. {
  44.   enabled_ = config.enabled;
  45.   ROS_INFO("pluglib: GridLayer: %s, enabled_=%d", __func__, enabled_);
  46. }
  47.  
  48. static struct timeval tv = {0};
  49. void GridLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x,
  50.                                            double* min_y, double* max_x, double* max_y)
  51. {
  52.   struct timeval tv_now;
  53.   gettimeofday(&tv_now, NULL);
  54.   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 );
  55.   memcpy(&tv, &tv_now, sizeof(tv));

  56.   if (!enabled_)
  57.     return;
  58.   
  59.   if (flag == 0)
  60.   {
  61.      //flag = 1;
  62.   }
  63.   else     return;
  64.  
  65.   double mark_x = robot_x + cos(robot_yaw);
  66.   double mark_y = robot_y + sin(robot_yaw);
  67.   unsigned int mx;
  68.   unsigned int my;
  69.   
  70.   ROS_INFO("pluglib: GridLayer: %s. pose=%lf %lf %lf, mark=%lf %lf ", __func__, robot_x, robot_y, robot_yaw, mark_x, mark_y);
  71.   if(worldToMap(mark_x, mark_y, mx, my)){
  72.      setCost(mx, my,LETHAL_OBSTACLE);
  73.          ROS_INFO("pluglib: SetCost: %lf, %lf --> %d, %d", mark_x, mark_y, mx, my);
  74.   }
  75.   
  76.   *min_x = std::min(*min_x, mark_x);
  77.   *min_y = std::min(*min_y, mark_y);
  78.   *max_x = std::max(*max_x, mark_x);
  79.   *max_y = std::max(*max_y, mark_y);
  80.   
  81.   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);
  82. }
  83.  
  84. void GridLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i,
  85.                                           int max_j)
  86. {
  87.   ROS_INFO("pluglib: GridLayer: %s enabled_=%d", __func__, enabled_);
  88.   if (!enabled_)
  89.     return;
  90.  
  91.  
  92.   ROS_INFO("pluglib: GridLayer: %s rect=%d %d -> %d %d", __func__, min_j, min_i, max_j, max_i);
  93.   for (int j = min_j; j < max_j; j++)
  94.   {
  95.     for (int i = min_i; i < max_i; i++)
  96.     {
  97.       int index = getIndex(i, j);
  98.       //if (costmap_[index] == NO_INFORMATION) continue;
  99.       master_grid.setCost(i, j, costmap_[index]);
  100.       ROS_INFO(" SetCost: %d --> %d, %d", costmap_[index], i, j);
  101.     }
  102.   }
  103. }
  104.  
  105. } // end namespace

阅读(10084) | 评论(0) | 转发(0) |
给主人留下些什么吧!~~
评论热议
请登录后评论。

登录 注册