现在的代码如下:#include "obstacle_avoidance.hpp"
// 辅助函数,将geometry_msgs::msg::Polygon转换为Shape数组
std::vector<ObstacleAvoidance::Shape> ObstacleAvoidance::convertPolygonToShapes(const geometry_msgs::msg::Polygon &polygon)
{
std::vector<ObstacleAvoidance::Shape> shapes;
for (const auto &point : polygon.points)
{
ObstacleAvoidance::Shape s;
s.x = point.x;
s.y = point.y;
s.z = point.z;
shapes.emplace_back(s);
}
return shapes;
}
// 辅助函数:将Shape向量转换为Polygon消息
geometry_msgs::msg::Polygon ObstacleAvoidance::convertShapesToPolygon(const std::vector<Shape> &shapes)
{
geometry_msgs::msg::Polygon polygon;
for (const auto &shape : shapes)
{
geometry_msgs::msg::Point32 point;
point.x = shape.x;
point.y = shape.y;
point.z = shape.z;
polygon.points.emplace_back(point);
}
return polygon;
}
// 辅助函数 位姿变换
// void ObstacleAvoidance::PoseTransform(std::vector<ObstacleAvoidance::Shape> &shapes, const KX::Pose &pose)
// {
// for (auto &shape : shapes)
// {
// KX::Pos point(shape.x, shape.y, shape.z);
// // point = pose * point;
// point = pose.pos() + point;
// shape.x = point.x();
// shape.y = point.y();
// shape.z = point.z();
// }
// }
// 辅助函数 位姿变换
// std::vector<ObstacleAvoidance::Shape> ObstacleAvoidance::transformShapes(const std::vector<ObstacleAvoidance::Shape> &shapes, const KX::Pose &pose)
// {
// std::vector<ObstacleAvoidance::Shape> transformed;
// for (const auto &s : shapes)
// {
// KX::Pos p(s.x, s.y, s.z);
// p = p + pose.pos();
// transformed.push_back({static_cast<float>(p.x()), static_cast<float>(p.y()), static_cast<float>(p.z())});
// }
// return transformed;
// }
// 位姿变换函数
std::vector<ObstacleAvoidance::Shape> ObstacleAvoidance::transformShapes(
const std::vector<ObstacleAvoidance::Shape> &shapes,
const KX::Pose &pose)
{
std::vector<ObstacleAvoidance::Shape> transformed;
const double yaw = pose.ori().yaw();
const double cos_yaw = cos(yaw);
const double sin_yaw = sin(yaw);
for (const auto &s : shapes)
{
// 旋转后平移
double x_rot = s.x * cos_yaw - s.y * sin_yaw;
double y_rot = s.x * sin_yaw + s.y * cos_yaw;
transformed.push_back({static_cast<float>(x_rot + pose.pos().x()),
static_cast<float>(y_rot + pose.pos().y()),
static_cast<float>(s.z + pose.pos().z())});
}
return transformed;
}
// 辅助函数:创建扩展包围盒
geometry_msgs::msg::Polygon ObstacleAvoidance::calExtensionPolygon(
const geometry_msgs::msg::Polygon &base_polygon,
double expand_front, double expand_back,
double expand_left, double expand_right)
{
// 计算基础包围盒
double min_x = std::numeric_limits<double>::max();
double max_x = std::numeric_limits<double>::lowest();
double min_y = std::numeric_limits<double>::max();
double max_y = std::numeric_limits<double>::lowest();
for (const auto &point : base_polygon.points)
{
min_x = std::min(min_x, static_cast<double>(point.x));
max_x = std::max(max_x, static_cast<double>(point.x));
min_y = std::min(min_y, static_cast<double>(point.y));
max_y = std::max(max_y, static_cast<double>(point.y));
}
// 应用扩展
min_x -= expand_back;
max_x += expand_front;
min_y -= expand_right;
max_y += expand_left;
// 创建矩形包围盒
geometry_msgs::msg::Polygon expanded_polygon;
geometry_msgs::msg::Point32 p;
expanded_polygon.points.reserve(4);
p.x = min_x;
p.y = min_y;
expanded_polygon.points.emplace_back(p); // 右下
p.x = min_x;
p.y = max_y;
expanded_polygon.points.emplace_back(p); // 左下
p.x = max_x;
p.y = max_y;
expanded_polygon.points.emplace_back(p); // 左上
p.x = max_x;
p.y = min_y;
expanded_polygon.points.emplace_back(p); // 右上
return expanded_polygon;
}
// 安全系数函数(离停止区域越近,安全系数越低)
double ObstacleAvoidance::calSafetyFactor(
const KX::Pos &obstacle_pos_in_world,
const KX::Pose &car_pose,
const geometry_msgs::msg::Polygon &base_car_polygon,
double stop_offset_front, double stop_offset_back,
double stop_offset_left, double stop_offset_right,
double dec_offset_front, double dec_offset_back,
double dec_offset_left, double dec_offset_right)
{
// 障碍物位置转换到车体坐标系
// KX::Pos obstacle_in_car = obstacle_pos_in_world - car_pose;
KX::Pos obstacle_in_car = obstacle_pos_in_world - car_pose.pos();
const double yaw = car_pose.ori().yaw();
const double cos_yaw = cos(-yaw); // 逆向旋转
const double sin_yaw = sin(-yaw);
// 应用旋转
double x_rot = obstacle_in_car.x() * cos_yaw - obstacle_in_car.y() * sin_yaw;
double y_rot = obstacle_in_car.x() * sin_yaw + obstacle_in_car.y() * cos_yaw;
obstacle_in_car.setX(x_rot);
obstacle_in_car.setY(y_rot);
// 计算基础包围盒
double min_x = std::numeric_limits<double>::max();
double max_x = std::numeric_limits<double>::lowest();
double min_y = std::numeric_limits<double>::max();
double max_y = std::numeric_limits<double>::lowest();
for (const auto &point : base_car_polygon.points)
{
min_x = std::min(min_x, static_cast<double>(point.x));
max_x = std::max(max_x, static_cast<double>(point.x));
min_y = std::min(min_y, static_cast<double>(point.y));
max_y = std::max(max_y, static_cast<double>(point.y));
}
// 计算停止区域边界
double stop_min_x = min_x - stop_offset_back;
double stop_max_x = max_x + stop_offset_front;
double stop_min_y = min_y - stop_offset_right;
double stop_max_y = max_y + stop_offset_left;
// 计算减速区域边界
double dec_min_x = min_x - dec_offset_back;
double dec_max_x = max_x + dec_offset_front;
double dec_min_y = min_y - dec_offset_right;
double dec_max_y = max_y + dec_offset_left;
// 检查障碍物是否在停止包围盒内
if (obstacle_in_car.x() >= stop_min_x && obstacle_in_car.x() <= stop_max_x &&
obstacle_in_car.y() >= stop_min_y && obstacle_in_car.y() <= stop_max_y)
{
return 0.0; // 安全系数为0
}
// 检查障碍物是否在减速包围盒外
if (obstacle_in_car.x() < dec_min_x || obstacle_in_car.x() > dec_max_x ||
obstacle_in_car.y() < dec_min_y || obstacle_in_car.y() > dec_max_y)
{
return 1.0; // 安全系数为1
}
// 障碍物在减速包围盒内,计算到停止包围盒的距离
double dx = 0.0, dy = 0.0;
if (obstacle_in_car.x() < stop_min_x)
{
dx = stop_min_x - obstacle_in_car.x();
}
else if (obstacle_in_car.x() > stop_max_x)
{
dx = obstacle_in_car.x() - stop_max_x;
}
if (obstacle_in_car.y() < stop_min_y)
{
dy = stop_min_y - obstacle_in_car.y();
}
else if (obstacle_in_car.y() > stop_max_y)
{
dy = obstacle_in_car.y() - stop_max_y;
}
double dist_to_stop = std::sqrt(dx * dx + dy * dy);
// 障碍物在减速包围盒内,计算到减速包围盒的距离
double dist_to_dec = 0.0;
if (obstacle_in_car.x() < dec_min_x)
{
dist_to_dec = dec_min_x - obstacle_in_car.x();
}
else if (obstacle_in_car.x() > dec_max_x)
{
dist_to_dec = obstacle_in_car.x() - dec_max_x;
}
else if (obstacle_in_car.y() < dec_min_y)
{
dist_to_dec = dec_min_y - obstacle_in_car.y();
}
else if (obstacle_in_car.y() > dec_max_y)
{
dist_to_dec = obstacle_in_car.y() - dec_max_y;
}
else
{
// 计算到最近减速区域边界的距离
double dx_min = std::min(
std::abs(obstacle_in_car.x() - dec_min_x),
std::abs(obstacle_in_car.x() - dec_max_x));
double dy_min = std::min(
std::abs(obstacle_in_car.y() - dec_min_y),
std::abs(obstacle_in_car.y() - dec_max_y));
dist_to_dec = std::min(dx_min, dy_min);
}
// 计算(相对)减速区域宽度
double dec_width = dist_to_dec + dist_to_stop;
// 安全系数计算
double ratio = (dec_width > 0) ? (dist_to_stop / dec_width) : 0;
// 凹函数:s = 1 - (x/D)^2
double safety_factor = 1.0 - ratio * ratio;
return std::clamp(safety_factor, 0.0, 1.0); // 限制在[0,1]范围
}
// 碰撞检测函数
void ObstacleAvoidance::GJKCheckCollision(
std::vector<Planning::Bezier3RoutePoint> &all_points,
int current_index,
const kx_topic::msg::PerceptionState &perception_state,
double current_speed,
double ¤t_detect_distance_out,
std::vector<std::string> ¤t_active_io_obstacle_out,
Planning::CollisionInfo &response_collision_out)
{
// 获取当前路径段配置的活动IO障碍物列表
if (current_index >= 0 && current_index < static_cast<int>(all_points.size()))
{
current_active_io_obstacle_out = all_points[current_index].belong_route.obstacle_active_io_obstacles;
}
else
{
current_active_io_obstacle_out.clear();
}
// 将活动的IO障碍物添加到当前活动列表中(如果它们不在列表中)
for (size_t i = 0; i < planning_->param_data_force_active_io_obstacles_.size(); i++)
{
if (std::find(current_active_io_obstacle_out.begin(),
current_active_io_obstacle_out.end(),
planning_->param_data_force_active_io_obstacles_[i]) == current_active_io_obstacle_out.end())
{ // 没有就添加
current_active_io_obstacle_out.emplace_back(planning_->param_data_force_active_io_obstacles_[i]);
}
}
// 过滤掉不在强制活动列表中的障碍物
std::vector<std::string>::iterator it_end =
std::remove_if(current_active_io_obstacle_out.begin(), current_active_io_obstacle_out.end(),
[this](std::string io_obstacle) -> bool
{
return std::find(planning_->param_data_force_active_io_obstacles_.begin(), planning_->param_data_force_active_io_obstacles_.end(), io_obstacle) != planning_->param_data_force_active_io_obstacles_.end();
});
current_active_io_obstacle_out.assign(current_active_io_obstacle_out.begin(), it_end);
// 碰撞相应初始化
response_collision_out.type = CollisionInfo::Type::NoCollision;
response_collision_out.limit_speed = INFINITY;
response_collision_out.car_distance = INFINITY;
response_collision_out.effect_io_obstacles.clear();
response_collision_out.car_pose_in_map.setPose(KX::Pos(0, 0, 0), KX::Ori(0, 0, 0));
response_collision_out.obstacle_point_in_map.setXYZ(0, 0, 0);
response_collision_out.obstacle_point_in_current_car.setXYZ(0, 0, 0);
response_collision_out.obstacle_point_in_future_car.setXYZ(0, 0, 0);
response_collision_out.car_polygon_in_future_car.points.clear();
response_collision_out.car_polygon_in_map.points.clear();
response_collision_out.car_stop_polygon_in_map.points.clear();
response_collision_out.car_limit_polygon_in_map.points.clear();
// 路径有效性检查
if (!all_points.size() || current_index < 0 || current_index >= (int)all_points.size())
return;
// 运动方向判断(点积)
// bool is_car_front_move = all_points[current_index].dir.dot(planning_->topic_result_pose_fusion_dir2d_) > 0;
// 获取包围盒参数
const double dec_offset_front = planning_->get_parameter(planning_->PARAM_NAME_GJK_DEC_OFFSET_FRONT).as_double();
const double dec_offset_back = planning_->get_parameter(planning_->PARAM_NAME_GJK_DEC_OFFSET_BACK).as_double();
const double dec_offset_left = planning_->get_parameter(planning_->PARAM_NAME_GJK_DEC_OFFSET_LEFT).as_double();
const double dec_offset_right = planning_->get_parameter(planning_->PARAM_NAME_GJK_DEC_OFFSET_RIGHT).as_double();
// const double dec_offset_top = planning_->get_parameter(planning_->PARAM_NAME_GJK_DEC_OFFSET_TOP).as_double();
// const double dec_offset_bottom = planning_->get_parameter(planning_->PARAM_NAME_GJK_DEC_OFFSET_BOTTOM).as_double();
const double stop_offset_front = planning_->get_parameter(planning_->PARAM_NAME_GJK_STOP_OFFSET_FRONT).as_double();
const double stop_offset_back = planning_->get_parameter(planning_->PARAM_NAME_GJK_STOP_OFFSET_BACK).as_double();
const double stop_offset_left = planning_->get_parameter(planning_->PARAM_NAME_GJK_STOP_OFFSET_LEFT).as_double();
const double stop_offset_right = planning_->get_parameter(planning_->PARAM_NAME_GJK_STOP_OFFSET_RIGHT).as_double();
// const double stop_offset_top = planning_->get_parameter(planning_->PARAM_NAME_GJK_STOP_OFFSET_TOP).as_double();
// const double stop_offset_bottom = planning_->get_parameter(planning_->PARAM_NAME_GJK_STOP_OFFSET_BOTTOM).as_double();
/* ****雷达碰撞**** */
if (planning_->get_parameter(planning_->PARAM_NAME_TRACK_BEZIER3_COLLISION_IS_ENABLE_ROUTE_FORECAST).as_bool())
{
// 请空路径点的碰撞状态
for (auto &point : all_points)
{
point.collision_limit_speed = INFINITY;
point.collision_driving_brake_request = 0;
}
/* ****预测碰撞**** */
size_t forward_look_ahead_points_ = planning_->get_parameter(planning_->PARAM_NAME_GJK_FORWARD_LOOK_AHEAD_POINTS).as_int(); // 前向探测点数
double speed_threshold_ = planning_->get_parameter(planning_->PARAM_NAME_GJK_SPEED_THRESHOLD).as_double(); // 速度阈值,超过该阈值就减速
double percentage_of_deceleration_ = planning_->get_parameter(planning_->PARAM_NAME_GJK_PERCENTAGE_OF_DECELERATION).as_double(); // 减速百分比
double safe_stop_distance_ = planning_->get_parameter(planning_->PARAM_NAME_GJK_SAFE_STOP_DISTANCE).as_double(); // 安全停车距离
std::vector<size_t> check_indices;
if (current_index == 0)
{
// 起始点之后x点
size_t end_index = std::min(all_points.size(), current_index + forward_look_ahead_points_);
for (size_t i = current_index; i <= end_index; i++)
{
check_indices.emplace_back(i);
}
}
else
{
// 非起始点
size_t target_index = current_index + forward_look_ahead_points_ - 1;
if (target_index < all_points.size())
{
check_indices.emplace_back(target_index);
}
// 终点之前x点
if (target_index >= all_points.size())
{
for (size_t i = current_index; i < all_points.size(); i++)
{
check_indices.emplace_back(i);
}
}
}
bool prediction_collition_detected = false;
double min_prediction_limit_speed = INFINITY;
size_t collision_index = SIZE_MAX;
for (size_t index : check_indices)
{
// 获取该路径点
auto &point = all_points[index];
// 车辆外形
geometry_msgs::msg::Polygon base_car_polygon = point.belong_route.is_polygon_without_fork
? perception_state.car_polygon_2d_without_fork
: perception_state.car_polygon_2d;
// 未来车辆位姿
// KX::Pose future_car_pose(KX::Pose(point.pos.x(), point.pos.y(), 0.0, 0.0, 0.0,
// is_car_front_move ? point.dir.theta() : point.dir.theta() + M_PI));
KX::Pose future_car_pose(KX::Pose(point.pos.x(), point.pos.y(), 0.0, 0.0, 0.0, point.dir.theta()));
// 创建停止包围盒
auto stop_polygon = calExtensionPolygon(base_car_polygon,
// all_points[current_index].belong_route.obstacle_stop_offset_front,
// all_points[current_index].belong_route.obstacle_stop_offset_back,
// all_points[current_index].belong_route.obstacle_stop_offset_left,
// all_points[current_index].belong_route.obstacle_stop_offset_right);
stop_offset_front,
stop_offset_back,
stop_offset_left,
stop_offset_right);
// 包围盒坐标系变换
std::vector<Shape> stop_polygon_shapes = convertPolygonToShapes(stop_polygon);
auto transformed_stop_polygon = transformShapes(stop_polygon_shapes, future_car_pose);
// 检测与所有障碍物的碰撞
bool stop_collision = false;
for (const auto &obstacle : perception_state.obstacles)
{
std::vector<Shape> obstacle_shapes = convertPolygonToShapes(obstacle.polygon);
auto transformed_obstacle_polygon = transformShapes(obstacle_shapes, future_car_pose);
if (gjk(transformed_stop_polygon.data(), transformed_obstacle_polygon.data(), transformed_stop_polygon.size(), transformed_obstacle_polygon.size()))
{
stop_collision = true;
break;
}
}
if (stop_collision) // 预测到碰撞,立即减速
{
prediction_collition_detected = true;
collision_index = index;
// 降速
if ((all_points[collision_index].collision_limit_speed ||
all_points[collision_index - 2 / (size_t)planning_->track_bezier3_waypoints_interval_m_].collision_limit_speed) > speed_threshold_) // 碰撞点速度规划的速度 > 阈值
{
double safe_speed = current_speed * percentage_of_deceleration_;
min_prediction_limit_speed = std::min(min_prediction_limit_speed, safe_speed);
}
break;
}
}
if (prediction_collition_detected && current_speed > speed_threshold_) // 更新路径点限速
{
// 计算安全停车位置
double collision_distance = all_points[collision_index].distance;
double stop_distance = collision_distance - safe_stop_distance_;
// 查找停车索引
size_t stop_index = collision_index;
for (int i = collision_index; i >= static_cast<int>(current_index); i--)
{
if (all_points[i].distance <= stop_distance)
{
stop_index = i;
break;
}
}
for (size_t k = current_index; k < stop_index; k++)
{
if (min_prediction_limit_speed < all_points[k].collision_limit_speed)
{
all_points[k].collision_limit_speed = min_prediction_limit_speed;
}
}
response_collision_out.limit_speed = min_prediction_limit_speed;
}
/* ****预测碰撞结束**** */
double min_collision_distance = INFINITY;
KX::Pose collision_pose;
KX::Pos collision_point;
double global_limit_speed = INFINITY;
// double global_speed_change_distance = current_detect_distance_out;
size_t start_index = (current_index == 0) ? 0 : current_index - 1;
for (size_t i = start_index; i < all_points.size(); i++)
{
const double distance_diff = all_points[i].distance - all_points[current_index].distance;
// 超出检测距离 , break
if (distance_diff > current_detect_distance_out)
break;
// 车辆外形
geometry_msgs::msg::Polygon base_car_polygon = all_points[i].belong_route.is_polygon_without_fork
? perception_state.car_polygon_2d_without_fork
: perception_state.car_polygon_2d;
// 未来车辆位姿
// KX::Pose future_car_pose(KX::Pose(all_points[i].pos.x(), all_points[i].pos.y(), 0.0, 0.0, 0.0,
// is_car_front_move ? all_points[i].dir.theta() : all_points[i].dir.theta() + M_PI));
KX::Pose future_car_pose(KX::Pose(all_points[i].pos.x(), all_points[i].pos.y(), 0.0, 0.0, 0.0, all_points[i].dir.theta()));
// 创建减速、停止包围盒
auto dec_polygon = calExtensionPolygon(base_car_polygon,
dec_offset_front,
dec_offset_back,
// all_points[current_index].belong_route.obstacle_limit_offset_left,
// all_points[current_index].belong_route.obstacle_limit_offset_right);
dec_offset_left,
dec_offset_right);
auto stop_polygon = calExtensionPolygon(base_car_polygon,
// all_points[current_index].belong_route.obstacle_stop_offset_front,
// all_points[current_index].belong_route.obstacle_stop_offset_back,
// all_points[current_index].belong_route.obstacle_stop_offset_left,
// all_points[current_index].belong_route.obstacle_stop_offset_right);
stop_offset_front,
stop_offset_back,
stop_offset_left,
stop_offset_right);
// 包围盒变换到世界坐标系
std::vector<Shape> dec_polygon_shapes = convertPolygonToShapes(dec_polygon);
auto transformed_dec_polygon = transformShapes(dec_polygon_shapes, future_car_pose);
std::vector<Shape> stop_polygon_shapes = convertPolygonToShapes(stop_polygon);
auto transformed_stop_polygon = transformShapes(stop_polygon_shapes, future_car_pose);
// 检测与所有障碍物的碰撞
bool stop_collision = false;
bool dec_collision = false;
KX::Pos current_collision_point;
// double current_collision_distance = INFINITY;
double point_limit_speed = INFINITY;
// double point_speed_change_distance = current_detect_distance_out;
for (const auto &obstacle : perception_state.obstacles)
{
// 转换障碍物外形-Shape
std::vector<Shape> obstacle_shapes = convertPolygonToShapes(obstacle.polygon);
auto transformed_obstacle = transformShapes(obstacle_shapes, future_car_pose);
// 检测停止区域碰撞
if (gjk(transformed_stop_polygon.data(), transformed_obstacle.data(),
transformed_stop_polygon.size(), transformed_obstacle.size()))
{
stop_collision = true;
current_collision_point = KX::Pos(obstacle.center_x, obstacle.center_y, obstacle.center_z);
point_limit_speed = 0; // 立即停止
// point_speed_change_distance = 0;
break; // 停止碰撞优先级最高
}
// 检测减速区域碰撞
if (gjk(transformed_dec_polygon.data(), transformed_obstacle.data(),
transformed_dec_polygon.size(), transformed_obstacle.size()))
{
dec_collision = true;
current_collision_point = KX::Pos(obstacle.center_x, obstacle.center_y, obstacle.center_z);
// 计算安全系数(离停止区域越近,安全系数越低)0-1
double safety_factor = calSafetyFactor(
current_collision_point, future_car_pose,
base_car_polygon,
// all_points[current_index].belong_route.obstacle_stop_offset_front,
// all_points[current_index].belong_route.obstacle_stop_offset_back,
// all_points[current_index].belong_route.obstacle_stop_offset_left,
// all_points[current_index].belong_route.obstacle_stop_offset_right,
stop_offset_front,
stop_offset_back,
stop_offset_left,
stop_offset_right,
dec_offset_front,
dec_offset_back,
// all_points[current_index].belong_route.obstacle_limit_offset_left,
// all_points[current_index].belong_route.obstacle_limit_offset_right);
dec_offset_left,
dec_offset_right);
double current_limit_speed = current_speed * safety_factor;
// 限速处理
if (current_limit_speed < point_limit_speed)
{
point_limit_speed = current_limit_speed;
}
}
}
// 如果有碰撞,更新碰撞信息
if (stop_collision || dec_collision)
{
// 更新全局碰撞信息
if (point_limit_speed < global_limit_speed)
{
global_limit_speed = point_limit_speed;
collision_pose = future_car_pose;
collision_point = current_collision_point;
min_collision_distance = distance_diff;
response_collision_out.car_stop_polygon_in_map = convertShapesToPolygon(transformed_stop_polygon);
response_collision_out.car_limit_polygon_in_map = convertShapesToPolygon(transformed_dec_polygon);
std::vector<Shape> base_car_shapes = convertPolygonToShapes(base_car_polygon);
auto transfromed_base_car_polygon = transformShapes(base_car_shapes, future_car_pose);
response_collision_out.car_polygon_in_map = convertShapesToPolygon(transfromed_base_car_polygon);
// 更新碰撞相应信息
response_collision_out.type = CollisionInfo::Type::LidarCollision;
response_collision_out.limit_speed = global_limit_speed;
response_collision_out.car_distance = min_collision_distance;
response_collision_out.car_pose_in_map = collision_pose;
response_collision_out.obstacle_point_in_map = collision_point;
response_collision_out.obstacle_point_in_current_car = collision_point - planning_->topic_result_pose_fusion_;
response_collision_out.obstacle_point_in_future_car = collision_point - collision_pose;
response_collision_out.car_polygon_in_future_car = base_car_polygon;
}
}
}
/* ****近距离碰撞保护**** */
if (planning_->get_parameter(planning_->PARAM_NAME_TRACK_BEZIER3_COLLISION_IS_USE_NEAR_PROTECT).as_bool())
{
// 获取车体外形
const auto &base_car_polygon = all_points[current_index].belong_route.is_polygon_without_fork
? perception_state.car_polygon_2d_without_fork
: perception_state.car_polygon_2d;
// 创建减速、停止包围盒 重复计算?
auto dec_polygon = calExtensionPolygon(base_car_polygon,
dec_offset_front,
dec_offset_back,
// all_points[current_index].belong_route.obstacle_limit_offset_left,
// all_points[current_index].belong_route.obstacle_limit_offset_right);
dec_offset_left,
dec_offset_right);
auto stop_polygon = calExtensionPolygon(base_car_polygon,
// all_points[current_index].belong_route.obstacle_stop_offset_front,
// all_points[current_index].belong_route.obstacle_stop_offset_back,
// all_points[current_index].belong_route.obstacle_stop_offset_left,
// all_points[current_index].belong_route.obstacle_stop_offset_right);
stop_offset_front,
stop_offset_back,
stop_offset_left,
stop_offset_right);
// 包围盒变换到世界坐标系
std::vector<Shape> dec_polygon_shapes = convertPolygonToShapes(dec_polygon);
auto transformed_dec_polygon = transformShapes(dec_polygon_shapes, planning_->topic_result_pose_fusion_);
std::vector<Shape> stop_polygon_shapes = convertPolygonToShapes(stop_polygon);
auto transformed_stop_polygon = transformShapes(stop_polygon_shapes, planning_->topic_result_pose_fusion_);
// 检测与所有障碍物的碰撞
bool stop_collision = false;
bool dec_collision = false;
KX::Pos collision_point;
double near_limit_speed = INFINITY;
// double near_speed_change_distance = 0;
for (const auto &obstacle : perception_state.obstacles)
{
// 转换障碍物形状
std::vector<Shape> obstacle_shapes = convertPolygonToShapes(obstacle.polygon);
auto transformed_obstacle = transformShapes(obstacle_shapes, planning_->topic_result_pose_fusion_);
// 检测停止区域碰撞
if (gjk(transformed_stop_polygon.data(), transformed_obstacle.data(),
transformed_stop_polygon.size(), transformed_obstacle.size()))
{
stop_collision = true;
collision_point = KX::Pos(obstacle.center_x, obstacle.center_y, obstacle.center_z);
break; // 停止碰撞优先级最高
}
// 检测减速碰撞
if (gjk(transformed_dec_polygon.data(), transformed_obstacle.data(),
transformed_dec_polygon.size(), transformed_obstacle.size()))
{
dec_collision = true;
collision_point = KX::Pos(obstacle.center_x, obstacle.center_y, obstacle.center_z);
// 计算安全系数
double safety_factor = calSafetyFactor(
collision_point, planning_->topic_result_pose_fusion_,
base_car_polygon,
// all_points[current_index].belong_route.obstacle_stop_offset_front,
// all_points[current_index].belong_route.obstacle_stop_offset_back,
// all_points[current_index].belong_route.obstacle_stop_offset_left,
// all_points[current_index].belong_route.obstacle_stop_offset_right,
stop_offset_front,
stop_offset_back,
stop_offset_left,
stop_offset_right,
dec_offset_front,
dec_offset_back,
// all_points[current_index].belong_route.obstacle_limit_offset_left,
// all_points[current_index].belong_route.obstacle_limit_offset_right);
dec_offset_left,
dec_offset_right);
//
double current_limit_speed = safety_factor * current_speed;
// 限速处理
if (current_limit_speed < near_limit_speed)
{
near_limit_speed = current_limit_speed;
}
}
}
// 处理碰撞响应
if (stop_collision || dec_collision)
{
const double brake_value = planning_->get_parameter(planning_->PARAM_NAME_TRACK_BEZIER3_COLLISION_IO_COLLISION_DRIVING_BRAKE).as_double();
// 设置全路径停车
for (auto &point : all_points)
{
if (stop_collision)
{
point.collision_limit_speed = 0;
}
else if (dec_collision)
{
// if (near_limit_speed < point.collision_limit_speed)
// {
// point.collision_limit_speed = near_limit_speed;
// }
point.collision_limit_speed = std::min(point.collision_limit_speed, near_limit_speed);
}
point.collision_driving_brake_request = brake_value;
}
// 更新碰撞相应信息
if (stop_collision || (dec_collision && near_limit_speed < response_collision_out.limit_speed))
{
response_collision_out.car_stop_polygon_in_map = convertShapesToPolygon(transformed_stop_polygon);
response_collision_out.car_limit_polygon_in_map = convertShapesToPolygon(transformed_dec_polygon);
std::vector<Shape> base_car_shapes = convertPolygonToShapes(base_car_polygon);
auto transfromed_base_car_polygon = transformShapes(base_car_shapes, planning_->topic_result_pose_fusion_);
response_collision_out.car_polygon_in_map = convertShapesToPolygon(transfromed_base_car_polygon);
response_collision_out.type = CollisionInfo::Type::LidarCollision;
response_collision_out.limit_speed = stop_collision ? 0 : near_limit_speed;
response_collision_out.car_distance = 0;
response_collision_out.car_pose_in_map = planning_->topic_result_pose_fusion_;
response_collision_out.obstacle_point_in_map = collision_point;
response_collision_out.obstacle_point_in_current_car = collision_point - planning_->topic_result_pose_fusion_;
response_collision_out.obstacle_point_in_future_car = collision_point - planning_->topic_result_pose_fusion_;
response_collision_out.car_polygon_in_future_car = base_car_polygon;
}
}
}
/* ****雷达碰撞结束**** */
/* ****IO碰撞**** */
bool io_collision_detected = false;
for (const auto &io_obstacle : current_active_io_obstacle_out)
{
if (std::find(perception_state.occur_io_obstacles.begin(),
perception_state.occur_io_obstacles.end(),
io_obstacle) != perception_state.occur_io_obstacles.end())
{
response_collision_out.effect_io_obstacles.emplace_back(io_obstacle);
io_collision_detected = true;
}
}
if (io_collision_detected)
{
const double brake_value = planning_->get_parameter(planning_->PARAM_NAME_TRACK_BEZIER3_COLLISION_IO_COLLISION_DRIVING_BRAKE).as_double();
const double io_limit_speed = 0; // IO避障要求完全停止
// 设置全径停车
for (auto &point : all_points)
{
if (io_limit_speed < point.collision_limit_speed)
{
point.collision_limit_speed = io_limit_speed;
}
point.collision_driving_brake_request = brake_value;
}
// 更新碰撞状态
response_collision_out.type = CollisionInfo::Type::IoCollision;
}
/* ****IO碰撞结束**** */
}
}
/* ****************************3d_gjk算法实现步骤**************************** */
/* 绕x轴旋转一个点 传入旋转角度,单位度*/
ObstacleAvoidance::Shape ObstacleAvoidance::Shape::rot_x(float rot_x)
{
float rad = rot_x * M_PI / 180.0f;
float cos_rad = cos(rad);
float sin_rad = sin(rad);
Shape return_shape;
return_shape.x = x;
return_shape.y = y * cos_rad - z * sin_rad;
return_shape.z = y * sin_rad + z * cos_rad;
return return_shape;
}
/* 绕y轴旋转一个点 传入旋转角度,单位度*/
ObstacleAvoidance::Shape ObstacleAvoidance::Shape::rot_y(float rot_y)
{
float rad = rot_y * M_PI / 180.0f;
float cos_rad = cos(rad);
float sin_rad = sin(rad);
Shape return_shape;
return_shape.x = x * cos_rad + z * sin_rad;
return_shape.y = y;
return_shape.z = z * cos_rad - x * sin_rad;
return return_shape;
}
/* 绕z轴旋转一个点 传入旋转角度,单位度*/
ObstacleAvoidance::Shape ObstacleAvoidance::Shape::rot_z(float rot_z)
{
float rad = rot_z * M_PI / 180.0f;
float cos_rad = cos(rad);
float sin_rad = sin(rad);
Shape return_shape;
return_shape.x = x * cos_rad - y * sin_rad;
return_shape.y = x * sin_rad + y * cos_rad;
return_shape.z = z;
return return_shape;
}
/* 对形状(向量)取反 */
ObstacleAvoidance::Shape ObstacleAvoidance::Shape::negate()
{
Shape result;
result.x = -x;
result.y = -y;
result.z = -z;
return result;
}
/* 两个形状(向量)的点积 */
float ObstacleAvoidance::Shape::dot(Shape d)
{
float dotproduct;
dotproduct = d.x * x + d.y * y + d.z * z;
return dotproduct;
}
/* 两个形状(向量)的叉积 */
ObstacleAvoidance::Shape ObstacleAvoidance::Shape::cross(Shape d)
{
Shape crossproduct;
crossproduct.x = y * d.z - z * d.y;
crossproduct.y = z * d.x - x * d.z;
crossproduct.z = x * d.y - y * d.x;
return crossproduct;
}
/* 点积函数 */
float dot_prod(float x1, float y1, float z1, float x2, float y2, float z2)
{
return (x1 * x2 + y1 * y2 + z1 * z2);
}
/* 叉积函数 a x (b x c) = b (a·c) - c (a·b)*/
ObstacleAvoidance::Shape triple_cross_prod(ObstacleAvoidance::Shape a, ObstacleAvoidance::Shape b, ObstacleAvoidance::Shape c)
{
ObstacleAvoidance::Shape triple_return;
triple_return.x = b.x * a.dot(c) - c.x * a.dot(b);
triple_return.y = b.y * a.dot(c) - c.y * a.dot(b);
triple_return.z = b.z * a.dot(c) - c.z * a.dot(b);
return triple_return;
}
/* 计算质心函数 dim_a为A的顶点数*/
ObstacleAvoidance::Shape mass_middle_point(ObstacleAvoidance::Shape *A, int dim_a)
{
ObstacleAvoidance::Shape mm;
mm.x = 0;
mm.y = 0;
mm.z = 0;
for (int i = 0; i < dim_a; i++)
{
mm.x += A[i].x;
mm.y += A[i].y;
mm.z += A[i].z;
}
mm.x /= dim_a;
mm.y /= dim_a;
mm.z /= dim_a;
return mm;
}
/* Support 函数
传入:形状A, 形状B, 形状A和B的元素数量, 搜索的反方向
输出:单纯型点 */
ObstacleAvoidance::Shape support_function(ObstacleAvoidance::Shape *A, ObstacleAvoidance::Shape *B, int dim_a, int dim_b, ObstacleAvoidance::Shape sd)
{
float max_val1 = -INFINITY;
int max_index1 = -1;
float dotp = 0.0;
// 在形状A上沿sd方向找最远点
for (int i = 0; i < dim_a; i++)
{
dotp = dot_prod(A[i].x, A[i].y, A[i].z, sd.x, sd.y, sd.z);
if (dotp > max_val1)
{
max_val1 = dotp;
max_index1 = i;
}
}
// 在形状B上沿-sd方向找最远点
float max_val2 = -INFINITY;
int max_index2 = -1;
for (int i = 0; i < dim_b; i++)
{
dotp = dot_prod(B[i].x, B[i].y, B[i].z, -sd.x, -sd.y, -sd.z);
if (dotp > max_val2)
{
max_val2 = dotp;
max_index2 = i;
}
}
ObstacleAvoidance::Shape return_shape;
return_shape.x = A[max_index1].x - B[max_index2].x;
return_shape.y = A[max_index1].y - B[max_index2].y;
return_shape.z = A[max_index1].z - B[max_index2].z;
return return_shape;
}
/* 向单纯形添加点 */
void ObstacleAvoidance::Simplex::add(Shape simplex_add)
{
int i = 3;
// 从后往前找到第一个空位
while (i >= 0)
{
if (x[i] != -INFINITY)
{
break;
}
i--;
}
i++;
// 在找到的位置添加新点
x[i] = simplex_add.x;
y[i] = simplex_add.y;
z[i] = simplex_add.z;
}
/* 初始化单纯型列表 */
void ObstacleAvoidance::Simplex::set_zero(void)
{
for (int i = 0; i < 4; i++)
{
// 将所有值设为-INFINITY避免无效数据
x[i] = -INFINITY;
y[i] = -INFINITY;
z[i] = -INFINITY;
}
}
/* 获取最后添加的点 */
ObstacleAvoidance::Shape ObstacleAvoidance::Simplex::getLast(void)
{
Shape return_last;
int i = 3;
while (i >= 0)
{
if (x[i] != -INFINITY)
{
break;
}
i--;
}
return_last.x = x[i];
return_last.y = y[i];
return_last.z = z[i];
return return_last;
}
/* 获取指定索引(id = 0-3)的点的x/y/z值 */
ObstacleAvoidance::Shape ObstacleAvoidance::Simplex::get(int id)
{
Shape return_shape;
if (id < 0 || id > 3)
{
std::cout << "无法获取索引为 " << id << " 的单纯形点" << std::endl;
}
return_shape.x = x[id];
return_shape.y = y[id];
return_shape.z = z[id];
return return_shape;
}
/* 计算单纯形中的有效点数 */
int ObstacleAvoidance::Simplex::size(void)
{
int count = 0;
for (int i = 0; i < 4; i++)
{
if (x[i] != -INFINITY)
{
count++;
}
}
return count;
}
/* 从单纯形中删除指定点 */
void ObstacleAvoidance::Simplex::del(int id) // id = 1、2、3、4
{
// 安全检查:计算当前有效点数
int simplex_elements = 0;
for (int i = 0; i < 4; i++)
{
if (x[i] != -INFINITY)
{
simplex_elements++;
}
}
if (simplex_elements < 4 || id == 4) // 无法删除最后添加的点
{
std::cout << "错误 -> 单纯形元素数: " << simplex_elements << " | 要删除的id: " << id << std::endl;
}
else
{
// 缓存形状数据
float cx[3], cy[3], cz[3];
int c = 0;
id--; // 调整索引从0开始
// 备份除删除点外的所有点
for (int i = 0; i < 4; i++)
{
if (i != id)
{
cx[c] = x[i];
cy[c] = y[i];
cz[c] = z[i];
c++;
}
x[i] = -INFINITY;
y[i] = -INFINITY;
z[i] = -INFINITY;
}
// 恢复备份的点
for (int i = 0; i < 3; i++)
{
x[i] = cx[i];
y[i] = cy[i];
z[i] = cz[i];
}
}
}
/* 检查原点是否在单纯形内 */
bool containsOrigin(ObstacleAvoidance::Simplex &simplex, ObstacleAvoidance::Shape &d)
{
const float EPSILON = 1e-5f;
ObstacleAvoidance::Shape a = simplex.getLast();
ObstacleAvoidance::Shape a0 = a.negate(); // AO向量
switch (simplex.size())
{
case 2:
{ // 线段情况
ObstacleAvoidance::Shape b = simplex.get(0);
ObstacleAvoidance::Shape ab = {b.x - a.x, b.y - a.y, b.z - a.z};
// 计算垂直方向
if (ab.dot(a0) > EPSILON)
{
d = triple_cross_prod(ab, a0, ab);
}
else
{
d = a0;
}
return false;
}
case 3:
{ // 三角形情况
ObstacleAvoidance::Shape b = simplex.get(1);
ObstacleAvoidance::Shape c = simplex.get(0);
ObstacleAvoidance::Shape ab = {b.x - a.x, b.y - a.y, b.z - a.z};
ObstacleAvoidance::Shape ac = {c.x - a.x, c.y - a.y, c.z - a.z};
ObstacleAvoidance::Shape abc = ab.cross(ac); // 三角形法线
// 检查边缘区域
ObstacleAvoidance::Shape abc_c_ac = abc.cross(ac);
if (abc_c_ac.dot(a0) > EPSILON)
{
if (ac.dot(a0) > EPSILON)
{
d = triple_cross_prod(ac, a0, ac);
}
else
{
if (ab.dot(a0) > EPSILON)
{
d = triple_cross_prod(ab, a0, ab);
}
else
{
d = a0;
}
}
}
else
{
ObstacleAvoidance::Shape ab_c_abc = ab.cross(abc);
if (ab_c_abc.dot(a0) > EPSILON)
{
if (ab.dot(a0) > EPSILON)
{
d = triple_cross_prod(ab, a0, ab);
}
else
{
d = a0;
}
}
else
{
// 原点在三角形上方或下方
if (abc.dot(a0) > EPSILON)
{
d = abc;
}
else
{
d = abc.negate();
}
}
}
return false;
}
case 4:
{ // 四面体情况
ObstacleAvoidance::Shape p_d = simplex.get(0); // 点d
ObstacleAvoidance::Shape p_c = simplex.get(1); // 点c
ObstacleAvoidance::Shape p_b = simplex.get(2); // 点b
// 计算边向量
ObstacleAvoidance::Shape ac = {p_c.x - a.x, p_c.y - a.y, p_c.z - a.z};
ObstacleAvoidance::Shape ab = {p_b.x - a.x, p_b.y - a.y, p_b.z - a.z};
ObstacleAvoidance::Shape ad = {p_d.x - a.x, p_d.y - a.y, p_d.z - a.z};
// 计算各面的法向量
ObstacleAvoidance::Shape abc = ac.cross(ab);
float v_abc = abc.dot(a0);
ObstacleAvoidance::Shape abd = ab.cross(ad);
float v_abd = abd.dot(a0);
ObstacleAvoidance::Shape acd = ad.cross(ac);
float v_acd = acd.dot(a0);
// 使用相对容差值
float max_abs = fmax(fabs(v_abc), fmax(fabs(v_abd), fabs(v_acd)));
float rel_epsilon = EPSILON * fmax(1.0f, max_abs);
// 检查原点是否在四面体内
if (v_abc > -rel_epsilon && v_abd > -rel_epsilon && v_acd > -rel_epsilon)
{
return true;
}
// 确定要移除的点
if (v_abc < v_abd && v_abc < v_acd)
{
simplex.del(1); // 移除点c
d = (v_abc > rel_epsilon) ? abc : abc.negate();
}
else if (v_abd < v_acd)
{
simplex.del(2); // 移除点b
d = (v_abd > rel_epsilon) ? abd : abd.negate();
}
else
{
simplex.del(3); // 移除点d
d = (v_acd > rel_epsilon) ? acd : acd.negate();
}
return false;
}
default:
// 不应该到达这里
d = a0;
return false;
}
}
// gjk主函数
bool ObstacleAvoidance::gjk(Shape *A, Shape *B, int dim_a, int dim_b)
{
const int MAX_ITERATIONS = 100;
const float EPSILON = 1e-5f;
Simplex simplex;
// 计算质心
Shape mma = mass_middle_point(A, dim_a);
Shape mmb = mass_middle_point(B, dim_b);
// 初始方向 (B质心指向A质心)
Shape d;
d.x = mmb.x - mma.x;
d.y = mmb.y - mma.y;
d.z = mmb.z - mma.z;
// 避免零向量
if (fabs(d.x) < EPSILON && fabs(d.y) < EPSILON && fabs(d.z) < EPSILON)
{
d.x = 1.0f;
}
// 规范化方向向量
float len = sqrt(d.x * d.x + d.y * d.y + d.z * d.z);
if (len > EPSILON)
{
d.x /= len;
d.y /= len;
d.z /= len;
}
// 获取第一个支撑点
Shape s = support_function(A, B, dim_a, dim_b, d);
simplex.add(s);
// 初始搜索方向指向原点
d = s.negate();
int iterations = 0;
while (iterations < MAX_ITERATIONS)
{
iterations++;
// 获取新支撑点
s = support_function(A, B, dim_a, dim_b, d);
// 检查新点是否在搜索方向的后方
float dot_product = s.dot(d);
if (dot_product < -EPSILON)
{
return false; // 明确在后方,无碰撞
}
// 检查点是否非常接近原点
float dist_sq = s.x * s.x + s.y * s.y + s.z * s.z;
if (dist_sq < EPSILON)
{
return true; // 非常接近原点,视为碰撞
}
// 添加新点到单纯形
simplex.add(s);
// 检查原点是否在单纯形内
if (containsOrigin(simplex, d))
{
return true;
}
// 检查搜索方向是否接近零
float dir_len_sq = d.x * d.x + d.y * d.y + d.z * d.z;
if (dir_len_sq < EPSILON)
{
// 方向向量接近零,可能表示原点在单纯形边界上
return true;
}
}
// 达到最大迭代次数仍未收敛
return false;
}
。测试情况是:在rviz中,障碍物相对于原点显示,但不是相对于车体现
最新发布