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

bool ChooseAccessibleBalls::canMove(float x, float y){
	return true;

//	  ROS_INFO("enter canMove");
	  // Get a copy of the current costmap to test. (threadsafe)
	  costmap_2d::Costmap2D costmap;
//	  ROS_INFO("aaa");
	  if(costmap_ros != NULL)
		  costmap_ros->getCostmapCopy( costmap );

//	  ROS_INFO("bbb");
	  // Coordinate transform.
	  unsigned int cell_x, cell_y;
	  if( !costmap.worldToMap( x, y, cell_x, cell_y )){
	    return false;
	  }

	  double cost = double( costmap.getCost( cell_x, cell_y ));
	  if(cost <= 127){
		  return true;
	  }
	  else{
		  return false;
	  }
}

inline bool MapGrid::updatePathCell (MapCell* current_cell, MapCell* check_cell,
                                     const costmap_2d::Costmap2D& costmap)
{

    //if the cell is an obstacle set the max path distance
    unsigned char cost = costmap.getCost (check_cell->cx, check_cell->cy);

    if (! getCell (check_cell->cx, check_cell->cy).within_robot &&
            (cost == costmap_2d::LETHAL_OBSTACLE ||
             cost == costmap_2d::INSCRIBED_INFLATED_OBSTACLE ||
             cost == costmap_2d::NO_INFORMATION))
    {
        check_cell->target_dist = obstacleCosts();
        return false;
    }

    double new_target_dist = current_cell->target_dist + 1;

    if (new_target_dist < check_cell->target_dist)
    {
        check_cell->target_dist = new_target_dist;
    }

    return true;
}

bool go(costmap_2d::Costmap2D &gradient_, costmap_2d::Costmap2D &original_, int i, int j, int cost)
{
	if(inMap(gradient_, i, j) == false){
		//ROS_INFO("salgo por no estar en mapa");
		return false;
	}

	if(original_.getCost( i, j) != 0 ){
		//ROS_INFO("salgo por ser obstaculo");
		return false;
	}

	if(gradient_.getCost( i, j) <= cost){
		//ROS_INFO("salgo por ser igual o menor");
		return false;
	}

	return true;
}

//mark the point of the costmap as local goal where global_plan first leaves the area (or its last point)
void MapGrid::setLocalGoal (const costmap_2d::Costmap2D& costmap,
                            const std::vector<geometry_msgs::PoseStamped>& global_plan)
{
    sizeCheck (costmap.getSizeInCellsX(), costmap.getSizeInCellsY());

    int local_goal_x = -1;
    int local_goal_y = -1;
    bool started_path = false;

    std::vector<geometry_msgs::PoseStamped> adjusted_global_plan;
    adjustPlanResolution (global_plan, adjusted_global_plan, costmap.getResolution());

    // skip global path points until we reach the border of the local map
    for (unsigned int i = 0; i < adjusted_global_plan.size(); ++i)
    {
        double g_x = adjusted_global_plan[i].pose.position.x;
        double g_y = adjusted_global_plan[i].pose.position.y;
        unsigned int map_x, map_y;

        if (costmap.worldToMap (g_x, g_y, map_x, map_y) && costmap.getCost (map_x, map_y) != costmap_2d::NO_INFORMATION)
        {
            local_goal_x = map_x;
            local_goal_y = map_y;
            started_path = true;
        }
        else
        {
            if (started_path)
            {
                break;
            }// else we might have a non pruned path, so we just continue
        }
    }

    if (!started_path)
    {
        ROS_ERROR ("None of the points of the global plan were in the local costmap, global plan points too far from robot");
        return;
    }

    queue<MapCell*> path_dist_queue;

    if (local_goal_x >= 0 && local_goal_y >= 0)
    {
        MapCell& current = getCell (local_goal_x, local_goal_y);
        costmap.mapToWorld (local_goal_x, local_goal_y, goal_x_, goal_y_);
        current.target_dist = 0.0;
        current.target_mark = true;
        path_dist_queue.push (&current);
    }

    computeTargetDistance (path_dist_queue, costmap);
}

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

//update what map cells are considered path based on the global_plan
void MapGrid::setTargetCells (const costmap_2d::Costmap2D& costmap,
                              const std::vector<geometry_msgs::PoseStamped>& global_plan)
{
    sizeCheck (costmap.getSizeInCellsX(), costmap.getSizeInCellsY());

    bool started_path = false;

    queue<MapCell*> path_dist_queue;

    std::vector<geometry_msgs::PoseStamped> adjusted_global_plan;
    adjustPlanResolution (global_plan, adjusted_global_plan, costmap.getResolution());

    if (adjusted_global_plan.size() != global_plan.size())
    {
        ROS_DEBUG ("Adjusted global plan resolution, added %zu points", adjusted_global_plan.size() - global_plan.size());
    }

    unsigned int i;

    // put global path points into local map until we reach the border of the local map
    for (i = 0; i < adjusted_global_plan.size(); ++i)
    {
        double g_x = adjusted_global_plan[i].pose.position.x;
        double g_y = adjusted_global_plan[i].pose.position.y;
        unsigned int map_x, map_y;

        if (costmap.worldToMap (g_x, g_y, map_x, map_y) && costmap.getCost (map_x, map_y) != costmap_2d::NO_INFORMATION)
        {
            MapCell& current = getCell (map_x, map_y);
            current.target_dist = 0.0;
            current.target_mark = true;
            path_dist_queue.push (&current);
            started_path = true;
        }
        else if (started_path)
        {
            break;
        }
    }

    if (!started_path)
    {
        ROS_ERROR ("None of the %d first of %zu (%zu) points of the global plan were in the local costmap and free",
                   i, adjusted_global_plan.size(), global_plan.size());
        return;
    }

    computeTargetDistance (path_dist_queue, costmap);
}

double getCost(float x, float y){
  // Get a copy of the current costmap to test. (threadsafe)
  costmap_2d::Costmap2D costmap;
  costmap_ros->getCostmapCopy( costmap );

  // Coordinate transform.
  unsigned int cell_x, cell_y;
  if( !costmap.worldToMap( x, y, cell_x, cell_y )){
	  return -1.0;
  }

  double cost = double( costmap.getCost( cell_x, cell_y ));
//  ROS_INFO(" world pose = (%f, %f)   map pose = (%d, %d)  cost =%f", x, y, cell_x, cell_y,  cost);
  return cost;
}

bool canMove(float x, float y){
	  // Get a copy of the current costmap to test. (threadsafe)
	  costmap_2d::Costmap2D costmap;
	  costmap_ros->getCostmapCopy( costmap );

	  // Coordinate transform.
	  unsigned int cell_x, cell_y;
	  if( !costmap.worldToMap( x, y, cell_x, cell_y )){
	 //   res.cost = -1.0;
	    return false;
	  }

	  double cost = double( costmap.getCost( cell_x, cell_y ));
	  ROS_INFO("cost = %f", cost);
	  if(cost <= 1){
		  return true;
	  }
	  else{
		  return false;
	  }
	 //ROS_INFO(" world pose = (%f, %f)   map pose = (%d, %d)  cost =%f", x, y, cell_x, cell_y,  cost);
}

  //update what map cells are considered path based on the global_plan
  void MapGrid::setPathCells(const costmap_2d::Costmap2D& costmap, const std::vector<geometry_msgs::PoseStamped>& global_plan){
    sizeCheck(costmap.getSizeInCellsX(), costmap.getSizeInCellsY(), costmap.getOriginX(), costmap.getOriginY());
    int local_goal_x = -1;
    int local_goal_y = -1;
    bool started_path = false;
    queue<MapCell*> path_dist_queue;
    queue<MapCell*> goal_dist_queue;
    for(unsigned int i = 0; i < global_plan.size(); ++i){
      double g_x = global_plan[i].pose.position.x;
      double g_y = global_plan[i].pose.position.y;
      unsigned int map_x, map_y;
      if(costmap.worldToMap(g_x, g_y, map_x, map_y) && costmap.getCost(map_x, map_y) != costmap_2d::NO_INFORMATION){
        MapCell& current = getCell(map_x, map_y);
        current.path_dist = 0.0;
        current.path_mark = true;
        path_dist_queue.push(&current);
        local_goal_x = map_x;
        local_goal_y = map_y;
        started_path = true;
      }
      else{
        if(started_path)
          break;
      }
    }

    if(local_goal_x >= 0 && local_goal_y >= 0){
      MapCell& current = getCell(local_goal_x, local_goal_y);
      costmap.mapToWorld(local_goal_x, local_goal_y, goal_x_, goal_y_);
      current.goal_dist = 0.0;
      current.goal_mark = true;
      goal_dist_queue.push(&current);
    }
    //compute our distances
    computePathDistance(path_dist_queue, costmap);
    computeGoalDistance(goal_dist_queue, costmap);
  }

void getMin(costmap_2d::Costmap2D gradient_, int &i, int &j)
{
	int iOut = i;
	int jOut = j;

	if(gradient_.getCost( i-1, j-1) < gradient_.getCost( iOut, jOut))
	{
		iOut = i - 1;
		jOut = j - 1;
	}
	if(gradient_.getCost( i-1, j) < gradient_.getCost( iOut, jOut))
	{
		iOut = i - 1;
		jOut = j;
	}
	if(gradient_.getCost( i-1, j+1) < gradient_.getCost( iOut, jOut))
	{
		iOut = i - 1;
		jOut = j + 1;
	}
	if(gradient_.getCost( i, j-1) < gradient_.getCost( iOut, jOut))
	{
		iOut = i;
		jOut = j - 1;
	}
	if(gradient_.getCost( i, j+1) < gradient_.getCost( iOut, jOut))
	{
		iOut = i;
		jOut = j + 1;
	}
	if(gradient_.getCost( i+1, j-1) < gradient_.getCost( iOut, jOut))
	{
		iOut = i + 1;
		jOut = j - 1;
	}
	if(gradient_.getCost( i+1, j) < gradient_.getCost( iOut, jOut))
	{
		iOut = i + 1;
		jOut = j;
	}
	if(gradient_.getCost( i+1, j+1) < gradient_.getCost( iOut, jOut))
	{
		iOut = i + 1;
		jOut = j + 1;
	}
	i = iOut;
	j = jOut;
}

    // 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");
    }

    bool TableApproaches::tableApprCallback(hrl_table_detect::GetTableApproaches::Request& req,
                                   hrl_table_detect::GetTableApproaches::Response& resp) {
        double pose_step = 0.01, start_dist = 0.25, max_dist = 1.2, min_cost = 250;
        double close_thresh = 0.10;


        // TODO should this be transformed?
        std::vector<geometry_msgs::Point> table_poly = req.table.points;
        geometry_msgs::PointStamped approach_pt = req.approach_pt;
        /*
        double xsize = 1.0, ysize = 1.0, xoff = 2.5, yoff = 0.0;
        geometry_msgs::Point pt; 
        pt.x = xoff-xsize/2; pt.y = yoff-ysize/2;
        table_poly.push_back(pt);
        pt.x = xoff+xsize/2; pt.y = yoff-ysize/2;
        table_poly.push_back(pt);
        pt.x = xoff+xsize/2; pt.y = yoff+ysize/2;
        table_poly.push_back(pt);
        pt.x = xoff-xsize/2; pt.y = yoff+ysize/2;
        table_poly.push_back(pt);
        geometry_msgs::PointStamped approach_pt;
        approach_pt.header.frame_id = "/base_link";
        approach_pt.header.stamp = ros::Time::now();
        approach_pt.point.x = 2.2; approach_pt.point.y = 0.3; 
        */

        costmap_ros->getCostmapCopy(costmap);
        world_model = new base_local_planner::CostmapModel(costmap);
        geometry_msgs::PoseArray valid_locs;
        valid_locs.header.frame_id = "/base_link";
        valid_locs.header.stamp = ros::Time::now();
        geometry_msgs::PoseArray invalid_locs;
        invalid_locs.header.frame_id = "/base_link";
        invalid_locs.header.stamp = ros::Time::now();
        geometry_msgs::PoseArray close_locs;
        close_locs.header.frame_id = "/base_link";
        close_locs.header.stamp = ros::Time::now();
        for(int i=0;i<4000;i++) {
            geometry_msgs::PoseStamped cpose, odom_pose;
            cpose.header.frame_id = "/base_link";
            cpose.header.stamp = ros::Time(0);
            cpose.pose.position.x = (rand()%8000) / 1000.0 -4 ; 
            cpose.pose.position.y = (rand()%8000) / 1000.0 - 4;
            double rot = (rand()%10000) / 10000.0 * 2 * 3.14;
            cpose.pose.orientation = tf::createQuaternionMsgFromYaw(rot);
            cout << cpose.pose.orientation.z << " " << cpose.pose.orientation.w << " " << rot << endl;
            tf_listener.transformPose("/odom_combined", cpose, odom_pose);
            //uint32_t x, y;
            //if(!costmap.worldToMap(odom_pose.pose.position.x, odom_pose.pose.position.y, x, y))
                //continue;
            Eigen::Vector3f pos(odom_pose.pose.position.x, odom_pose.pose.position.y, tf::getYaw(odom_pose.pose.orientation));
            //cout << x << ", " << y << ":" << curpt.point.x << "," << curpt.point.y << "$";
            //cout << double(costmap.getCost(x,y)) << endl;
            double foot_cost = footprintCost(pos, 1);
            if(foot_cost == 0) 
                valid_locs.poses.push_back(cpose.pose);
            else if(foot_cost != -1)
                close_locs.poses.push_back(cpose.pose);
            else 
                invalid_locs.poses.push_back(cpose.pose);
            cout << foot_cost << endl;
        }
        cout << costmap_ros->getRobotFootprint().size() << endl;
        valid_pub.publish(valid_locs);
        invalid_pub.publish(invalid_locs);
        close_pub.publish(close_locs);

        geometry_msgs::PoseArray dense_table_poses;
        dense_table_poses.header.frame_id = "/base_link";
        dense_table_poses.header.stamp = ros::Time::now();
        uint32_t i2;
        for(uint32_t i=0;i<table_poly.size();i++) {
            i2 = i+1;
            if(i2 == table_poly.size())
                i2 = 0;
            double diffx = table_poly[i2].x-table_poly[i].x;
            double diffy = table_poly[i2].y-table_poly[i].y;
            double len = std::sqrt(diffx*diffx + diffy*diffy);
            double ang = std::atan2(diffy, diffx) - 3.14/2;
            double incx = std::cos(ang)*0.01, incy = std::sin(ang)*0.01;
            for(double t=0;t<len;t+=pose_step) {
                double polyx = table_poly[i].x + t*diffx;
                double polyy = table_poly[i].y + t*diffy;
                geometry_msgs::PoseStamped test_pose, odom_test_pose;
                bool found_pose = false;
                for(int k=start_dist/0.01;k<max_dist/0.01;k++) {
                    test_pose.header.frame_id = "/base_link";
                    test_pose.header.stamp = ros::Time(0);
                    test_pose.pose.position.x = polyx + incx*k;
                    test_pose.pose.position.y = polyy + incy*k;
                    test_pose.pose.orientation = tf::createQuaternionMsgFromYaw(ang+3.14);
                    tf_listener.transformPose("/odom_combined", test_pose, odom_test_pose);
                    Eigen::Vector3f pos(odom_test_pose.pose.position.x, 
                                        odom_test_pose.pose.position.y, 
                                        tf::getYaw(odom_test_pose.pose.orientation));
                    double foot_cost = footprintCost(pos, 1.0);
                    // found a valid pose
                    if(foot_cost >= 0 && foot_cost <= min_cost) {
                        found_pose = true;
                        break;
//.........这里部分代码省略.........