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

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

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

  bool transformGlobalPlan(
      const tf::TransformListener& tf,
      const std::vector<geometry_msgs::PoseStamped>& global_plan,
      const tf::Stamped<tf::Pose>& global_pose,
      const costmap_2d::Costmap2D& costmap,
      const std::string& global_frame,
      std::vector<geometry_msgs::PoseStamped>& transformed_plan){
    const geometry_msgs::PoseStamped& plan_pose = global_plan[0];

    transformed_plan.clear();

    try {
      if (!global_plan.size() > 0) {
        ROS_ERROR("Received plan with zero length");
        return false;
      }

      // get plan_to_global_transform from plan frame to global_frame
      tf::StampedTransform plan_to_global_transform;
      tf.waitForTransform(global_frame, ros::Time::now(),
                          plan_pose.header.frame_id, plan_pose.header.stamp,
                          plan_pose.header.frame_id, ros::Duration(0.5));
      tf.lookupTransform(global_frame, ros::Time(),
                         plan_pose.header.frame_id, plan_pose.header.stamp, 
                         plan_pose.header.frame_id, plan_to_global_transform);

      //let's get the pose of the robot in the frame of the plan
      tf::Stamped<tf::Pose> robot_pose;
      tf.transformPose(plan_pose.header.frame_id, global_pose, robot_pose);

      //we'll discard points on the plan that are outside the local costmap
      double dist_threshold = std::max(costmap.getSizeInCellsX() * costmap.getResolution() / 2.0,
                                       costmap.getSizeInCellsY() * costmap.getResolution() / 2.0);

      unsigned int i = 0;
      double sq_dist_threshold = dist_threshold * dist_threshold;
      double sq_dist = 0;

      //we need to loop to a point on the plan that is within a certain distance of the robot
      while(i < (unsigned int)global_plan.size()) {
        double x_diff = robot_pose.getOrigin().x() - global_plan[i].pose.position.x;
        double y_diff = robot_pose.getOrigin().y() - global_plan[i].pose.position.y;
        sq_dist = x_diff * x_diff + y_diff * y_diff;
        if (sq_dist <= sq_dist_threshold) {
          break;
        }
        ++i;
      }

      tf::Stamped<tf::Pose> tf_pose;
      geometry_msgs::PoseStamped newer_pose;

      //now we'll transform until points are outside of our distance threshold
      while(i < (unsigned int)global_plan.size() && sq_dist <= sq_dist_threshold) {
        const geometry_msgs::PoseStamped& pose = global_plan[i];
        poseStampedMsgToTF(pose, tf_pose);
        tf_pose.setData(plan_to_global_transform * tf_pose);
        tf_pose.stamp_ = plan_to_global_transform.stamp_;
        tf_pose.frame_id_ = global_frame;
        poseStampedTFToMsg(tf_pose, newer_pose);

        transformed_plan.push_back(newer_pose);

        double x_diff = robot_pose.getOrigin().x() - global_plan[i].pose.position.x;
        double y_diff = robot_pose.getOrigin().y() - global_plan[i].pose.position.y;
        sq_dist = x_diff * x_diff + y_diff * y_diff;

        ++i;
      }
    }
    catch(tf::LookupException& ex) {
      ROS_ERROR("No Transform available Error: %s\n", ex.what());
      return false;
    }
    catch(tf::ConnectivityException& ex) {
      ROS_ERROR("Connectivity Error: %s\n", ex.what());
      return false;
    }
    catch(tf::ExtrapolationException& ex) {
      ROS_ERROR("Extrapolation Error: %s\n", ex.what());
      if (global_plan.size() > 0)
        ROS_ERROR("Global Frame: %s Plan Frame size %d: %s\n", global_frame.c_str(), (unsigned int)global_plan.size(), global_plan[0].header.frame_id.c_str());

      return false;
    }

    return true;
  }