参考:【ROS-Navigation】Costmap2D代价地图源码解读-静态层StaticLayer[1]
静态地图部分的主要内容是开启地图的订阅,以及bound和cost的更新。静态地图默认只更新一次,地图的边界也只更新一次,对于rolling地图,需要随着机器人的运动位置不断更新附近的cost,而对于非rolling地图则不需要。
一、初始化 StaticLayer::onInitialize
Costmap2DROS的构造函数会调用各Layer中的initialize函数,而initialize函数会调用onInitialize函数,真正的初始化工作在这里完成。
void StaticLayer::onInitialize() { ros::NodeHandle nh("~/" + name_), g_nh; nh.setCallbackQueue(&cbqueue_); g_nh.setCallbackQueue(&cbqueue_); current_ = true; global_frame_ = layered_costmap_->getGlobalFrameID(); std::string map_topic; nh.param("map_topic", map_topic, std::string("map")); nh.param("first_map_only", first_map_only_, false); nh.param("subscribe_to_updates", subscribe_to_updates_, false); nh.param("track_unknown_space", track_unknown_space_, true); nh.param("use_maximum", use_maximum_, false); int temp_lethal_threshold, temp_unknown_cost_value; nh.param("lethal_cost_threshold", temp_lethal_threshold, int(100)); nh.param("unknown_cost_value", temp_unknown_cost_value, int(-1)); nh.param("trinary_costmap", trinary_costmap_, true); lethal_threshold_ = std::max(std::min(temp_lethal_threshold, 100), 0); unknown_cost_value_ = temp_unknown_cost_value;
从参数服务器加载订阅静态地图的话题名称;默认静态地图只接收一次,不进行更新;以及默认追踪未知区域(主地图默认关闭)
// Only resubscribe if topic has changed // 只有当话题改变后才重新订阅 if (map_sub_.getTopic() != ros::names::resolve(map_topic)) { // we'll subscribe to the latched topic that the map server uses ROS_INFO("Requesting the map..."); map_sub_ = g_nh.subscribe(map_topic, 1, &StaticLayer::incomingMap, this); map_received_ = false; has_updated_data_ = false; ros::Rate r(10); //如果map_received_一直是false,则一直阻塞在这里,只有在接收到后进入回调函数更新为true后才会继续进行 while (!map_received_ && g_nh.ok()) { // ros::spinOnce(); cbqueue_.callAvailable(ros::WallDuration()); r.sleep(); } ROS_INFO("Received a %d X %d map at %f m/pix", getSizeInCellsX(), getSizeInCellsY(), getResolution());
订阅map_topic,回调函数:incomingMap,map_received_和has_updated_data_标志位设置为false(一旦收到地图,进入回调函数,它们会被置为true),没有接收到地图时,阻塞。
if (subscribe_to_updates_) { ROS_INFO("Subscribing to updates"); map_update_sub_ = g_nh.subscribe(map_topic + "_updates", 10, &StaticLayer::incomingUpdate, this); } } else { has_updated_data_ = true; } if (dsrv_) { delete dsrv_; } dsrv_ = new dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig>(nh); dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig>::CallbackType cb = boost::bind( &StaticLayer::reconfigureCB, this, _1, _2); dsrv_->setCallback(cb); }
二、回调函数 StaticLayer::incomingMap
获取接收到的静态地图的尺寸,当地图不随机器人移动时,若接收到的静态地图和主地图的尺寸/分辨率/起点不同,以接收到的地图为准,调整主地图的参数。
void StaticLayer::incomingMap(const nav_msgs::OccupancyGridConstPtr& new_map) { unsigned int size_x = new_map->info.width, size_y = new_map->info.height; ROS_DEBUG("Received a %d X %d map at %f m/pix", size_x, size_y, new_map->info.resolution); // resize costmap if size, resolution or origin do not match Costmap2D* master = layered_costmap_->getCostmap(); if (!layered_costmap_->isRolling() && (master->getSizeInCellsX() != size_x || master->getSizeInCellsY() != size_y || master->getResolution() != new_map->info.resolution || master->getOriginX() != new_map->info.origin.position.x || master->getOriginY() != new_map->info.origin.position.y)) { // Update the size of the layered costmap (and all layers, including this one) ROS_INFO("Resizing costmap to %d X %d at %f m/pix", size_x, size_y, new_map->info.resolution); layered_costmap_->resizeMap(size_x, size_y, new_map->info.resolution, new_map->info.origin.position.x, new_map->info.origin.position.y, true /* set size_locked to true, prevents reconfigureCb from overriding map size*/);
如果master map的尺寸、分辨率或原点与获取到的地图不匹配,重新设置master map
} else if (size_x_ != size_x || size_y_ != size_y || resolution_ != new_map->info.resolution || origin_x_ != new_map->info.origin.position.x || origin_y_ != new_map->info.origin.position.y) { // only update the size of the costmap stored locally in this layer ROS_INFO("Resizing static layer to %d X %d at %f m/pix", size_x, size_y, new_map->info.resolution); resizeMap(size_x, size_y, new_map->info.resolution, new_map->info.origin.position.x, new_map->info.origin.position.y);
若本层的数据和接收到的静态地图的参数不同时,继续以接收到的地图为准,调整本层参数。
} unsigned int index = 0; // initialize the costmap with static data for (unsigned int i = 0; i < size_y; ++i) { for (unsigned int j = 0; j < size_x; ++j) { unsigned char value = new_map->data[index]; costmap_[index] = interpretValue(value); ++index; } } map_frame_ = new_map->header.frame_id; // we have a new map, update full size of map x_ = y_ = 0; width_ = size_x_; height_ = size_y_; map_received_ = true; has_updated_data_ = true; // shutdown the map subscrber if firt_map_only_ flag is on if (first_map_only_) { ROS_INFO("Shutting down the map subscriber. first_map_only flag is on"); map_sub_.shutdown(); }
将接收到的静态地图数据复制到本层地图,复制过程中调用interpretValue函数,进行“翻译”,内容后述。并设置好地图坐标系中的原点x_、y_,以及地图尺寸和标志位。
若first_map_only_开启,则在第一次接收到地图后,话题将不再接收消息。
}
三、“翻译” StaticLayer::interpretValue
接收到的地图上:unknown_cost_value(默认为-1)为未知区域,lethal_cost_threshold(默认100)以上为致命障碍物
- unknown_cost_value_。默认-1。表示该栅格上的障碍物情况未知。它们称为地图上的未知区域。你当生成一张栅格图时,往往会存在未知区域。1)有效地图区域完全被障碍物包围,包圈圈外区域就会全是未知区域。2)地图探索不完全,漏出缝隙,而且机器人没进缝隙,缝隙外就存在未区域。对第一种情况,未知道那肯定是不能走了。对第二种情况,未知区域可能是能走的。
- lethal_cost_threshold。默认100。一旦栅格代价>=该值,认为该栅格代价是致命障碍LETHAL_OBSTACLE(254)。
- bool track_unknown_space_。是否要探索未知区域。默认true。未知区域上面已说,就是代价等于unknown_cost_value的栅格,这区域可能能走,可能不能走。所谓探索,指是在本层,栅格代价设为NO_INFORMATION(255),这语议还是表示未知,但要由上面层进一步修改为有效值代价。false时,赋值为FREE_SPACE(0)。也就是说,当不探索时,直接认为未知区域就没有障碍物。
- bool trinary_costmap_。默认true。用于设置是否对代价地图进行三值化。如果为true,代价地图应该被解释为只有3个值:空闲、占用、未知。不是否则直接使用其按比算出的代价值。
- bool use_maximum_。updateCosts时,是否取最大值,还是直接覆盖。默认false。
unsigned char StaticLayer::interpretValue(unsigned char value) { // check if the static value is above the unknown or lethal thresholds //如果追踪未知区域,且传入的参数代表未知,设置为NO_INFORMATION if (track_unknown_space_ && value == unknown_cost_value_) return NO_INFORMATION; //如果不追踪未知区域,且传入的参数代表未知,设置为FREE_SPACE else if (!track_unknown_space_ && value == unknown_cost_value_) return FREE_SPACE; //LETHAL_OBSTACLE else if (value >= lethal_threshold_) return LETHAL_OBSTACLE; //如果都不是,设置FREE_SPACE else if (trinary_costmap_) return FREE_SPACE; //如果以上都不是,返回对应比例的障碍值 double scale = (double) value / lethal_threshold_; return scale * LETHAL_OBSTACLE; }
变量cost是在该未知栅格,膨胀层传染出的代价。
track_unknown_space_ | inflate_unknown_ | 最终栅格代价 |
true | true | cost>FREE_SPACE,代价就设为cost |
true | false | cost≥INSCRIBED_INFLATED_OBSTACLE,代价才设为cost |
true | -- | 不满足以上两个条件,保持NO_INFORMATION |
false | -- | FREE_SPACE |
四、更新静态地图边界 StaticLayer::updateBounds
void StaticLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y, double* max_x, double* max_y) { if( !layered_costmap_->isRolling() ){ if (!map_received_ || !(has_updated_data_ || has_extra_bounds_)) return; }
若非rolling地图,在有地图数据更新时更新边界,否则,根据静态层更新的区域的边界更新传入的边界。
has_extra_bounds_初始值为false,而在第一次结束前,has_updated_data_被设置为false
且订阅话题在第一次接收到消息后关闭,故对于static layer,updateBounds只进行一次
Bounds的范围是整张地图的大小,在updateBounds过程中没有对静态地图层做任何的更新
useExtraBounds(min_x, min_y, max_x, max_y); double wx, wy; mapToWorld(x_, y_, wx, wy); *min_x = std::min(wx, *min_x); *min_y = std::min(wy, *min_y); mapToWorld(x_ + width_, y_ + height_, wx, wy); *max_x = std::max(wx, *max_x); *max_y = std::max(wy, *max_y); has_updated_data_ = false;
将map系中的起点(x_, y_)与终点(x_ + width_,, y_ + height_)转换到世界系,确保传入的bound能包含整个map在世界系中的范围。
}
由于默认不更新静态地图层,该层的bound将只更新一次。
五、更新静态地图代价 StaticLayer::updateCosts
这里将更新后的bound传入。
void StaticLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j) { if (!map_received_) return; if (!layered_costmap_->isRolling()) { // if not rolling, the layered costmap (master_grid) has same coordinates as this layer if (!use_maximum_) updateWithTrueOverwrite(master_grid, min_i, min_j, max_i, max_j); else updateWithMax(master_grid, min_i, min_j, max_i, max_j);
若不是rolling地图,那么直接将静态地图层bound范围内的内容合并到主地图,因为二者的尺寸也一样,updateWithTrueOverwrite和updateWithMax采用不同的合并策略。
} else { // If rolling window, the master_grid is unlikely to have same coordinates as this layer unsigned int mx, my; double wx, wy; // Might even be in a different frame geometry_msgs::TransformStamped transform; try { transform = tf_->lookupTransform(map_frame_, global_frame_, ros::Time(0)); } catch (tf2::TransformException ex) { ROS_ERROR("%s", ex.what()); return; } // Copy map data given proper transformations tf2::Transform tf2_transform; tf2::convert(transform.transform, tf2_transform); for (unsigned int i = min_i; i < max_i; ++i) { for (unsigned int j = min_j; j < max_j; ++j) { // Convert master_grid coordinates (i,j) into global_frame_(wx,wy) coordinates //master map坐标(i,j)→global坐标(wx,wy) layered_costmap_->getCostmap()->mapToWorld(i, j, wx, wy); // Transform from global_frame_ to map_frame_ tf2::Vector3 p(wx, wy, 0); p = tf2_transform*p; // Set master_grid with cell from map //global坐标p→静态地图坐标(mx, my) if (worldToMap(p.x(), p.y(), mx, my)) { if (!use_maximum_) master_grid.setCost(i, j, getCost(mx, my)); else master_grid.setCost(i, j, std::max(getCost(mx, my), master_grid.getCost(i, j))); } } }
若为rolling地图,则找到静态地图和global系之间的坐标转换,通过主地图→global→静态地图的转换过程,找到主地图的cell在静态地图上对应的cost,赋值给主地图。
} }
参考
- 【ROS-Navigation】Costmap2D代价地图源码解读-静态层StaticLayer https://blog.csdn.net/Neo11111/article/details/104849757