C++ Costmap2D::setCost方法代码示例

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);
  }
  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
    tf::StampedTransform transform;
    try
    {
      tf_->lookupTransform(map_frame_, global_frame_, ros::Time(0), transform);
    }
    catch (tf::TransformException ex)
    {
      ROS_ERROR("%s", ex.what());
      return;
    }
    // Copy map data given proper transformations
    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
        layered_costmap_->getCostmap()->mapToWorld(i, j, wx, wy);
        // Transform from global_frame_ to map_frame_
        tf::Point p(wx, wy, 0);
        p = transform(p);
        // Set master_grid with cell from map
        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)));
        }
      }
    }
  }
}

void upTo(costmap_2d::Costmap2D &gradient_){

	for (int varX = 0; varX < gradient_.getSizeInCellsX(); varX++) {
		for (int varY = 0; varY < gradient_.getSizeInCellsY(); varY++) {
			//if(gradient_.getCost(varX, varY) == 0)
				gradient_.setCost(varX, varY, DEF);
		}
	}
}

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

    const unsigned char* master_array = master_grid.getCharMap();

    for (int j = min_j; j < max_j; j++)
    {
      for (int i = min_i; i < max_i; i++)
      {

        if (clear_flag)
        {
          master_grid.setCost(i, j, FREE_SPACE);
        }
        else
        {
          int index = getIndex(i, j);

          if (costmap_[index] == NO_INFORMATION)
            continue;
        
          unsigned char old_cost = master_array[index];

          if (old_cost == NO_INFORMATION || old_cost < costmap_[index])
            master_grid.setCost(i, j, costmap_[index]);
	 }
       }
     }

     for (int i=0; i < getSizeInCellsX()*getSizeInCellsY(); i++)
       costmap_[i] == NO_INFORMATION;

     if (!obstacle_buffer.empty())
     {
       ROS_INFO("Costmap updated with ball obstacles.");
       obstacle_buffer.clear();
       std_srvs::Empty emptysrv;
       confirm_update_client.call(emptysrv);
     }

  }

void SimpleLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i,
                                          int max_j)
{
  if (!enabled_)
    return;
  unsigned int mx;
  unsigned int my;
  if(master_grid.worldToMap(mark_x_, mark_y_, mx, my)){
    master_grid.setCost(mx, my, LETHAL_OBSTACLE);
  }
}

void VirtualWallLayer::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]);
        }
    }
}

void SimpleLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i,
                                          int max_j)
{
  if (!enabled_)
    return;
  unsigned int mx;
  unsigned int my;
  if(pode_marcar){
  if(master_grid.worldToMap(mark_x_, mark_y_, mx, my)){
      for(int i = -1; i < 1; i++)
    master_grid.setCost(mx+5, my+i, LETHAL_OBSTACLE);
  }

  pode_marcar = false;
  }

 // ROS_INFO("Hi!");
}

void spread(costmap_2d::Costmap2D &gradient_, costmap_2d::Costmap2D &original_, int value, int i, int j, int startCellI, int startCellJ)
{

	gradient_.setCost(i, j, value);

	//ROS_INFO("(%d, %d) <- %d", i, j, value);

	value = value + 5;
	if(value>254)
		value=254;

	if(go(gradient_, original_, i-1,j, value))
		spread(gradient_, original_, value, i-1, j, startCellI, startCellJ);
	if(go(gradient_, original_, i,j-1, value))
		spread(gradient_, original_, value, i, j-1, startCellI, startCellJ);
	if(go(gradient_, original_, i+1,j, value))
		spread(gradient_, original_, value, i+1, j, startCellI, startCellJ);
	if(go(gradient_, original_, i,j+1, value))
		spread(gradient_, original_, value, i, j+1, startCellI, startCellJ);


}

    // method for applyting changes in this layer to global costmap
    void HANPLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j,
                                   int max_i, int max_j)
    {
        boost::recursive_mutex::scoped_lock lock(configuration_mutex_);

        if(!enabled_ || (lastTransformedHumans.size() == 0))
        {
            return;
        }

        // update map according to each human position
        for(auto human : lastTransformedHumans)
        {
            // get position of human in map
            unsigned int cell_x, cell_y;
            if(!master_grid.worldToMap(human.pose.position.x, human.pose.position.y, cell_x, cell_y))
            {
                ROS_ERROR("hanp_layer: no world coordinates for human at x:%f y:%f",
                               human.pose.position.x, human.pose.position.y);
                continue;
            }

            // general algorithm is to
            // create satic grids according to human posture, standing, sitting, walking
            // to calcuate cost use a sigmoid function
            // however 'sitting' posture is not implemented yet

            if(use_safety)
            {
                if(safety_grid == NULL)
                {
                    ROS_WARN_THROTTLE(THROTTLE_TIME, "hanp_layer: not updating costmap as safety_grid is empty");
                    return;
                }

                // apply safety grid according to position of each human
                auto size_x = (int)(safety_max / resolution), size_y = size_x;

                // by convention x is number of columns, y is number of rows
                for(int y = 0; y <= 2 * size_y; y++)
                {
                    for(int x = 0; x <= 2 * size_x; x++)
                    {
                        // add to current cost
                        auto cost = (double)master_grid.getCost(x + cell_x - size_x, y + cell_y - size_y)
                            + (double)(safety_grid[x + ((2 * size_x + 1) * y)] * safety_weight);
                        // detect overflow
                        if(cost > (int)costmap_2d::LETHAL_OBSTACLE)
                        {
                            cost = costmap_2d::LETHAL_OBSTACLE;
                        }

                        master_grid.setCost(x + cell_x - size_x, y + cell_y - size_y, (unsigned int)cost);
                    }
                }
            }

            if(use_visibility)
            {
                // calculate visibility grid
                double yaw = tf::getYaw(human.pose.orientation);
                auto visibility_grid = createVisibilityGrid(visibility_max, resolution,
                                                            costmap_2d::LETHAL_OBSTACLE, cos(yaw), sin(yaw));
                if(visibility_grid == NULL)
                {
                    ROS_WARN_THROTTLE(THROTTLE_TIME, "hanp_layer: not updating costmap as visibility_grid is empty");
                    return;
                }

                // apply the visibility grid
                auto size_x = (int)(visibility_max / resolution), size_y = size_x;

                for(int y = 0; y <= 2 * size_y; y++)
                {
                    for(int x = 0; x <= 2 * size_x; x++)
                    {
                        // add to current cost
                        auto cost = (double)master_grid.getCost(x + cell_x - size_x, y + cell_y - size_y)
                            + (double)(visibility_grid[x + ((2 * size_x + 1) * y)] * visibility_weight);
                        // detect overflow
                        if(cost > (int)costmap_2d::LETHAL_OBSTACLE)
                        {
                            cost = costmap_2d::LETHAL_OBSTACLE;
                        }

                        master_grid.setCost(x + cell_x - size_x, y + cell_y - size_y, (unsigned int)cost);
                    }
                }
            }

        }

        // ROS_DEBUG("hanp_layer: updated costs");
    }