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