修改点1:
void StaticLayer::incomingMap(cv::Mat map_info)
int k = 0;
for (unsigned int j = size_y - 1; j > 0; j--)
{
for (unsigned int i = 0; i < size_x; i++)
{
//todo:
//unsigned char value = 255;
///unsigned char value = 50; //待处理new_map->data[index]
//std::cout << "Please input number:" << std::endl;
//std::cin >> value;
//std::cout << "StaticLayer::incomingMap24...index = "<< index<<std::endl;
//std::cout << "1"<<std::endl
// std::cout << "map_info[" << index << "] = " << static_cast<int>(map_info.at<unsigned char>(i, j)) << std::endl;
//index = MAP_IDX(size_x,i,size_y - j - 1);
costmap_[index] = interpretValue((254 - map_info.at<unsigned char>(j, i)));
costmap_[index] = interpretValue((254 - map_info.at<unsigned char>(j, i)));
//std::cout << "255 - map_info[" << index << "] = " << static_cast<int>(254 - map_info.at<unsigned char>(i, j)) << std::endl;
++index;
}
}
修改点2:
void StaticLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
{
min_i = 0;
min_j = 0;
max_i = size_x_;
max_j =size_y_;
std::cout << "min_i" <<min_i<< std::endl;
std::cout << "min_j = " << min_j <<std::endl;
std::cout << "max_j = " << max_j<<std::endl;
std::cout << "max_i:" <<max_i << std::endl;
修改点3
void StaticLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y,
double* max_x, double* max_y)
{
*min_x = 0;
*min_y = 0;
*max_x = size_x_;
*max_y =size_y_;
}