eband_local_planner源码解析（3）conversions_and_types.h/cpp 解读

conversions_and_types.h/cpp 这两个文件比较简单, 定义了一些转换函数和泡泡结构体.

直接给出注释代码

conversions_and_types.h

/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2010, Willow Garage, Inc.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Willow Garage, Inc. nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 * Author: Christian Connette
 *********************************************************************/

#ifndef EBAND_CONVERSIONS_AND_TYPES_H_
#define EBAND_CONVERSIONS_AND_TYPES_H_

#include <ros/ros.h>

// msgs
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/Pose2D.h>
#include <geometry_msgs/PoseStamped.h>

#include <geometry_msgs/TransformStamped.h>

// transforms
#include <angles/angles.h>
// #include <tf/tf.h>
// #include <tf/transform_listener.h>
// #include <tf/transform_datatypes.h>
#include <tf2/convert.h>
#include <tf2/transform_datatypes.h>
#include <tf2/utils.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/buffer.h>

// costmap & geometry
#include <costmap_2d/costmap_2d_ros.h>

namespace eband_local_planner {

// typedefs

///<@brief defines a bubble - pose of center & radius of according hypersphere
///<(expansion)
// 论文核心内容, 泡泡
// 包括中心位姿和泡泡半径
struct Bubble {
  geometry_msgs::PoseStamped center;
  double expansion;
};

// 更新泡泡时向前和向后两个方向, 遍历时使用
enum AddAtPosition { add_front, add_back };

// functions

// pose - Quaternions,pose2D - euler angles
/**
 * @brief Converts a frame of type Pose to type Pose2D (mainly -> conversion of
 * orientation from quaternions to euler angles)
 * @param Pose which shall be converted
 * @param References to converted ROS Pose2D frmae
 */
// 3D位姿转2D位姿, 保留 x y 和 yaw
void PoseToPose2D(const geometry_msgs::Pose pose,
                  geometry_msgs::Pose2D &pose2D);

/**
 * @brief Converts a frame of type Pose to type Pose2D (mainly -> conversion of
 * orientation from euler angles to quaternions, -> z-coordinate is set to zero)
 * @param References to converted ROS Pose2D frame
 * @param Pose2D which shall be converted
 */
// 2D位姿转3D位姿
void Pose2DToPose(geometry_msgs::Pose &pose,
                  const geometry_msgs::Pose2D pose2D);

/**
 * @brief  Transforms the global plan of the robot from the planner frame to the
 * local frame. This replaces the transformGlobalPlan as defined in the
 * base_local_planner/goal_functions.h main difference is that it additionally
 * outputs counter indicating which part of the plan has been transformed.
 * @param tf A reference to a transform listener
 * @param global_plan The plan to be transformed
 * @param costmap A reference to the costmap being used so the window size for
 * transforming can be computed
 * @param global_frame The frame to transform the plan to
 * @param transformed_plan Populated with the transformed plan
 * @param number of start and end frame counted from the end of the global plan
 */
// 替换了 base_local_planner/goal_functions.h 的 transformGlobalPlan 函数
// 功能都是将规划路径从全局坐标系转到局部坐标系
// 这个重载函数新增了一个参数, 可以获取被转坐标系的路径在整个路径数组中的首末位置
bool transformGlobalPlan(
    const tf2_ros::Buffer &tf,
    const std::vector<geometry_msgs::PoseStamped> &global_plan,
    costmap_2d::Costmap2DROS &costmap, const std::string &global_frame,
    std::vector<geometry_msgs::PoseStamped> &transformed_plan,
    std::vector<int> &start_end_counts_from_end);

// 计算机器人的包围半径
/**
 * @brief Gets the footprint of the robot and computes the circumscribed radius
 * for the eband approach
 * // 这个函数根据 costmap_common_params.yaml 里机器人 footprint 来计算
 * // 计算长方形外接圆半径
 * @param costmap A reference to the costmap from which the radius is computed
 * @return radius in meters
 */
double getCircumscribedRadius(costmap_2d::Costmap2DROS &costmap);

}; // namespace eband_local_planner
#endif

conversions_and_types.cpp

/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2010, Willow Garage, Inc.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Willow Garage, Inc. nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 * Author: Christian Connette
 *********************************************************************/

#include <eband_local_planner/conversions_and_types.h>

namespace eband_local_planner {

// 3D 转 2D, 保留 x y yaw
void PoseToPose2D(const geometry_msgs::Pose pose,
                  geometry_msgs::Pose2D &pose2D) {
  // use tf-pkg to convert angles
  tf2::Transform pose_tf;

  // convert geometry_msgs::PoseStamped to tf2::Transform
  tf2::convert(pose, pose_tf);

  // now get Euler-Angles from pose_tf
  double useless_pitch, useless_roll, yaw;
  pose_tf.getBasis().getEulerYPR(yaw, useless_pitch, useless_roll);

  // normalize angle
  yaw = angles::normalize_angle(yaw);

  // and set to pose2D
  pose2D.x = pose.position.x;
  pose2D.y = pose.position.y;
  pose2D.theta = yaw;
}

// 2D 转 3D, 很简单
void Pose2DToPose(geometry_msgs::Pose &pose,
                  const geometry_msgs::Pose2D pose2D) {
  // use tf-pkg to convert angles
  tf2::Quaternion frame_quat;

  // transform angle from euler-angle to quaternion representation
  frame_quat.setRPY(0.0, 0.0, pose2D.theta);

  // set position
  pose.position.x = pose2D.x;
  pose.position.y = pose2D.y;
  pose.position.z = 0.0;

  // set quaternion
  pose.orientation.x = frame_quat.x();
  pose.orientation.y = frame_quat.y();
  pose.orientation.z = frame_quat.z();
  pose.orientation.w = frame_quat.w();
}

// 功能都是将规划路径从全局坐标系转到局部坐标系
bool transformGlobalPlan(
    const tf2_ros::Buffer &tf,
    const std::vector<geometry_msgs::PoseStamped> &global_plan,
    costmap_2d::Costmap2DROS &costmap, const std::string &global_frame,
    std::vector<geometry_msgs::PoseStamped> &transformed_plan,
    std::vector<int> &start_end_counts) {

  // global_plan[0] 是当前位置朝向目标点的路径中,下一步要到达的点
  // 从起点开始数
  // plan_pose 的 frame_id 就是全局坐标系 id, 下一步要用到
  const geometry_msgs::PoseStamped &plan_pose = global_plan[0];

  // initiate refernce variables
  transformed_plan.clear();

  try {
    if (global_plan.empty()) {
      ROS_ERROR("Recieved plan with zero length");
      return false;
    }

    geometry_msgs::TransformStamped transform;
    transform =
        tf.lookupTransform(global_frame, ros::Time(), plan_pose.header.frame_id,
                           plan_pose.header.stamp, plan_pose.header.frame_id);

    // let's get the pose of the robot in the frame of the plan
    geometry_msgs::TransformStamped robot_pose;
    robot_pose.transform.rotation.w = 1; // identity quaternion
    robot_pose.header.frame_id = costmap.getBaseFrameID();
    robot_pose.header.stamp = ros::Time();
    tf.transform(robot_pose, robot_pose, plan_pose.header.frame_id);
    // we'll keep points on the plan that are within the window that we're
    // looking at

    // 下面包括 while 循环在内的一连串操作
    // 首先计算局部代价地图大小
    // 之后通过计算的局部代价地图大小, 计算路径中每一个点到机器人的距离是不是在局部地图里
    // 最后只留在局部地图里的路径
    // 可以理解为局部地图"截断"了全局路径

    // 这个是局部代价地图的高和宽 在 local_costmap_params.yaml 里
    double dist_threshold =
        std::max(costmap.getCostmap()->getSizeInMetersX() / 2.0,
                 costmap.getCostmap()->getSizeInMetersY() / 2.0);

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

    // --- start - modification w.r.t. base_local_planner
    // initiate start_end_count
    std::vector<int> start_end_count;
    start_end_count.assign(2, 0);

    // we know only one direction and that is forward! - initiate search with
    // previous start_end_counts this is neccesserry to work with the sampling
    // based planners - path may severall time enter and leave moving window
    ROS_ASSERT((start_end_counts.at(0) > 0) &&
               (start_end_counts.at(0) <= (int)global_plan.size()));
    i = (unsigned int)global_plan.size() - (unsigned int)start_end_counts.at(0);
    // --- end - modification w.r.t. base_local_planner

    // 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() &&
           sq_dist > sq_dist_threshold) {
      double x_diff =
          robot_pose.transform.translation.x - global_plan[i].pose.position.x;
      double y_diff =
          robot_pose.transform.translation.y - global_plan[i].pose.position.y;
      sq_dist = x_diff * x_diff + y_diff * y_diff;

      // --- start - modification w.r.t. base_local_planner
      // not yet in reach - get next frame
      if (sq_dist > sq_dist_threshold)
        ++i;
      else
        // set counter for start of transformed intervall - from back as
        // beginning of plan might be prunned
        start_end_count.at(0) = (int)(((unsigned int)global_plan.size()) - i);
      // --- end - modification w.r.t. base_local_planner
    }

    // tf::Stamped<tf::Pose> tf_pose;
    tf2::Stamped<tf2::Transform> tf_pose, tf_transform;
    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) {
      double x_diff =
          robot_pose.transform.translation.x - global_plan[i].pose.position.x;
      double y_diff =
          robot_pose.transform.translation.y - global_plan[i].pose.position.y;
      sq_dist = x_diff * x_diff + y_diff * y_diff;

      const geometry_msgs::PoseStamped &pose = global_plan[i];
      // poseStampedMsgToTF(pose, tf_pose);
      // tf_pose.setData(transform * tf_pose);
      // tf_pose.stamp_ = transform.stamp_;
      tf2::convert(pose, tf_pose);
      tf2::convert(transform, tf_transform);
      tf_pose.setData(tf_transform * tf_pose);
      tf_pose.stamp_ = tf_transform.stamp_;
      tf_pose.frame_id_ = global_frame;
      tf2::toMsg(tf_pose, newer_pose);
      transformed_plan.push_back(newer_pose);

      // --- start - modification w.r.t. base_local_planner
      // set counter for end of transformed intervall - from back as beginning
      // of plan might be prunned
      start_end_count.at(1) = int(((unsigned int)global_plan.size()) - i);
      // --- end - modification w.r.t. base_local_planner

      ++i;
    }

    // --- start - modification w.r.t. base_local_planner
    // write to reference variable
    start_end_counts = start_end_count;
    // --- end - modification w.r.t. base_local_planner
  } catch (tf2::LookupException &ex) {
    ROS_ERROR("No Transform available Error: %s\n", ex.what());
    return false;
  } catch (tf2::ConnectivityException &ex) {
    ROS_ERROR("Connectivity Error: %s\n", ex.what());
    return false;
  } catch (tf2::ExtrapolationException &ex) {
    ROS_ERROR("Extrapolation Error: %s\n", ex.what());
    if (!global_plan.empty())
      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;
}

// 这个函数根据 costmap_common_params.yaml 里机器人 footprint 来计算
// 计算长方形外接圆半径
double getCircumscribedRadius(costmap_2d::Costmap2DROS &costmap) {
  std::vector<geometry_msgs::Point> footprint(costmap.getRobotFootprint());
  double max_distance_sqr = 0;
  for (size_t i = 0; i < footprint.size(); ++i) {
    geometry_msgs::Point &p = footprint[i];
    double distance_sqr = p.x * p.x + p.y * p.y;
    if (distance_sqr > max_distance_sqr) {
      max_distance_sqr = distance_sqr;
    }
  }

  // ROS_INFO_STREAM("max_distance_sqr: " << max_distance_sqr);
  return sqrt(max_distance_sqr);
}

} // namespace eband_local_planner