“#include "wall_detection.h"
// 全局变量定义
tf2_ros::Buffer* tfBuffer = nullptr;
ros::Publisher pointcloud_pub;
ros::Publisher lines_pub;
ros::Publisher normals_pub;
ros::Publisher nearest_point_pub;
ros::Publisher tf_points_pub;
ros::Publisher min_distance_pub;
ros::Publisher nearest_distance_pub; //最近距离
//
ros::Publisher left_front_nearest_distance_pub; // 左前侧垂直距离
ros::Publisher right_front_nearest_distance_pub; // 右前侧垂直距离
ros::Publisher left_back_nearest_distance_pub; // 左后侧垂直距离
ros::Publisher right_back_nearest_distance_pub; // 右后侧垂直距离
ros::Publisher foot_points_pub;//垂足
//
bool isLeftWall(const DetectedLine& wall) {
// 计算墙面中点Y坐标,大于0视为左侧墙面
return (wall.start.y + wall.end.y) / 2.0 > 0;
}
geometry_msgs::Point left_front_ref, left_back_ref;
geometry_msgs::Point right_front_ref, right_back_ref;
std::string choose_wall ;
bool getPointParameters(ros::NodeHandle& nh)
{
// 为每个点的每个分量设置默认值
double left_front_default_x = 0.0, left_front_default_y = 0.0, left_front_default_z = 0.0;
double left_back_default_x = 0.0, left_back_default_y = 0.0, left_back_default_z = 0.0;
double right_front_default_x = 0.0, right_front_default_y = 0.0, right_front_default_z = 0.0;
double right_back_default_x = 0.0, right_back_default_y = 0.0, right_back_default_z = 0.0;
std::string inital_choose = "all";
// 获取左前点参数
nh.param<double>("left_front_ref_x", left_front_ref.x, left_front_default_x);
nh.param<double>("left_front_ref_y", left_front_ref.y, left_front_default_y);
nh.param<double>("left_front_ref_z", left_front_ref.z, left_front_default_z);
// 获取左后点参数
nh.param<double>("left_back_ref_x", left_back_ref.x, left_back_default_x);
nh.param<double>("left_back_ref_y", left_back_ref.y, left_back_default_y);
nh.param<double>("left_back_ref_z", left_back_ref.z, left_back_default_z);
// 获取右前点参数
nh.param<double>("right_front_ref_x", right_front_ref.x, right_front_default_x);
nh.param<double>("right_front_ref_y", right_front_ref.y, right_front_default_y);
nh.param<double>("right_front_ref_z", right_front_ref.z, right_front_default_z);
// 获取右后点参数
nh.param<double>("right_back_ref_x", right_back_ref.x, right_back_default_x);
nh.param<double>("right_back_ref_y", right_back_ref.y, right_back_default_y);
nh.param<double>("right_back_ref_z", right_back_ref.z, right_back_default_z);
nh.param<std::string>("choose_wall",choose_wall, "inital_choose");
// 打印获取的值
ROS_INFO("Left Front Ref: (%.2f, %.2f, %.2f)",
left_front_ref.x, left_front_ref.y, left_front_ref.z);
ROS_INFO("Left Back Ref: (%.2f, %.2f, %.2f)",
left_back_ref.x, left_back_ref.y, left_back_ref.z);
ROS_INFO("Right Front Ref: (%.2f, %.2f, %.2f)",
right_front_ref.x, right_front_ref.y, right_front_ref.z);
ROS_INFO("Right Back Ref: (%.2f, %.2f, %.2f)",
right_back_ref.x, right_back_ref.y, right_back_ref.z);
ROS_INFO("Choose_wall_type: %s", choose_wall.c_str());
return true;
}
void setupStaticTFs()
{
static tf2_ros::StaticTransformBroadcaster static_broadcaster;
// 位于激光雷达左前方的tf坐标
geometry_msgs::TransformStamped left_tf;
left_tf.header.stamp = ros::Time::now();
//left_tf.header.stamp = ros::Time(0);
left_tf.header.frame_id = "base_footprint";
left_tf.child_frame_id = "laser_left_front";
left_tf.transform.translation.x = left_front_ref.x;
left_tf.transform.translation.y = left_front_ref.y;
left_tf.transform.translation.z = left_front_ref.z;
left_tf.transform.rotation.x = 0.0;
left_tf.transform.rotation.y = 0.0;
left_tf.transform.rotation.z = 0.0;
left_tf.transform.rotation.w = 1.0;
// 位于激光雷达右前方的tf坐标
geometry_msgs::TransformStamped right_tf;
right_tf.header.stamp = ros::Time::now();
//right_tf.header.stamp = ros::Time(0);
right_tf.header.frame_id = "base_footprint";
right_tf.child_frame_id = "laser_right_front";
right_tf.transform.translation.x = right_front_ref.x;
right_tf.transform.translation.y = right_front_ref.y;
right_tf.transform.translation.z = right_front_ref.z;
right_tf.transform.rotation.x = 0.0;
right_tf.transform.rotation.y = 0.0;
right_tf.transform.rotation.z = 0.0;
right_tf.transform.rotation.w = 1.0;
//左后方TF坐标
geometry_msgs::TransformStamped left_back_tf;
left_back_tf.header.stamp = ros::Time::now();
//left_back_tf.header.stamp = ros::Time(0);
left_back_tf.header.frame_id = "base_footprint";
left_back_tf.child_frame_id = "laser_left_back";
left_back_tf.transform.translation.x = left_back_ref.x;
left_back_tf.transform.translation.y = left_back_ref.y;
left_back_tf.transform.translation.z = left_back_ref.z;
left_back_tf.transform.rotation.x = 0.0;
left_back_tf.transform.rotation.y = 0.0;
left_back_tf.transform.rotation.z = 0.0;
left_back_tf.transform.rotation.w = 1.0;
// 右后方TF坐标
geometry_msgs::TransformStamped right_back_tf;
right_back_tf.header.stamp = ros::Time::now();
//right_back_tf.header.stamp = ros::Time(0);
right_back_tf.header.frame_id = "base_footprint";
right_back_tf.child_frame_id = "laser_right_back";
right_back_tf.transform.translation.x = right_back_ref.x;
right_back_tf.transform.translation.y = right_back_ref.y;
right_back_tf.transform.translation.z = right_back_ref.z;
right_back_tf.transform.rotation.x = 0.0;
right_back_tf.transform.rotation.y = 0.0;
right_back_tf.transform.rotation.z = 0.0;
right_back_tf.transform.rotation.w = 1.0;
static_broadcaster.sendTransform(left_tf);
static_broadcaster.sendTransform(right_tf);
static_broadcaster.sendTransform(left_back_tf);
static_broadcaster.sendTransform(right_back_tf);
}
double pointToLineDistance(const geometry_msgs::Point& point,
const geometry_msgs::Point& line_start,
const geometry_msgs::Point& line_end)
{
Eigen::Vector3f pt(point.x, point.y, point.z);
Eigen::Vector3f line_vec(line_end.x - line_start.x,
line_end.y - line_start.y,
line_end.z - line_start.z);
Eigen::Vector3f pt_vec(pt.x() - line_start.x,
pt.y() - line_start.y,
pt.z() - line_start.z);
double line_length = line_vec.norm();
if (line_length < 1e-6) {
return pt_vec.norm();
}
Eigen::Vector3f normalized_line = line_vec / line_length;
double projection = pt_vec.dot(normalized_line);
// 限制投影在直线范围内
projection = std::max(0.0, std::min(line_length, projection));
Eigen::Vector3f closest_point = Eigen::Vector3f(line_start.x, line_start.y, line_start.z) +
projection * normalized_line;
return (pt - closest_point).norm();
}
// 计算点到直线的垂足点
geometry_msgs::Point pointToLineProjection(const geometry_msgs::Point& point,
const geometry_msgs::Point& line_start,
const geometry_msgs::Point& line_end)
{
Eigen::Vector3f pt(point.x, point.y, point.z);
Eigen::Vector3f line_vec(line_end.x - line_start.x,
line_end.y - line_start.y,
line_end.z - line_start.z);
Eigen::Vector3f pt_vec(pt.x() - line_start.x,
pt.y() - line_start.y,
pt.z() - line_start.z);
double line_length = line_vec.norm();
if (line_length < 1e-6) {
return line_start; // 如果直线长度为零,返回起点
}
Eigen::Vector3f normalized_line = line_vec / line_length;
double projection = pt_vec.dot(normalized_line);
// 限制投影在直线范围内
projection = std::max(0.0, std::min(line_length, projection));
Eigen::Vector3f closest_point = Eigen::Vector3f(line_start.x, line_start.y, line_start.z) +
projection * normalized_line;
geometry_msgs::Point foot_point;
foot_point.x = closest_point.x();
foot_point.y = closest_point.y();
foot_point.z = closest_point.z();
return foot_point;
}
void publishDetectedLines(const std::vector<DetectedLine>& lines, const std::string& frame_id) {
visualization_msgs::Marker line_marker;
line_marker.header.frame_id = frame_id;
line_marker.header.stamp = ros::Time::now();
line_marker.ns = "detected_line";
line_marker.id = 0;
line_marker.type = visualization_msgs::Marker::LINE_LIST;
line_marker.pose.orientation.w = 1.0;
line_marker.scale.x = 0.15;
line_marker.color.r = 0.0;
line_marker.color.g = 0.0;
line_marker.color.b = 1.0;
line_marker.color.a = 1.0;
for (const auto& line : lines) {
geometry_msgs::Point p1, p2;
p1.x = line.start.x; p1.y = line.start.y; p1.z = line.start.z;
p2.x = line.end.x; p2.y = line.end.y; p2.z = line.end.z;
line_marker.points.push_back(p1);
line_marker.points.push_back(p2);
}
lines_pub.publish(line_marker);
visualization_msgs::MarkerArray normal_markers;
int id = 0;
for (const auto& line : lines) {
visualization_msgs::Marker normal_marker;
normal_marker.header.frame_id = frame_id;
normal_marker.header.stamp = ros::Time::now();
normal_marker.ns = "normals";
normal_marker.id = id++;
normal_marker.type = visualization_msgs::Marker::ARROW;
normal_marker.action = visualization_msgs::Marker::ADD;
normal_marker.pose.orientation.w = 1.0;
normal_marker.scale.x = 0.02;
normal_marker.scale.y = 0.04;
normal_marker.scale.z = 0.0;
normal_marker.color.r = 1.0;
normal_marker.color.g = 0.0;
normal_marker.color.b = 0.0;
normal_marker.color.a = 1.0;
geometry_msgs::Point mid_point;
mid_point.x = (line.start.x + line.end.x) / 2.0;
mid_point.y = (line.start.y + line.end.y) / 2.0;
mid_point.z = (line.start.z + line.end.z) / 2.0;
geometry_msgs::Point normal_end;
normal_end.x = mid_point.x + line.normal.normal_x * 0.5;
normal_end.y = mid_point.y + line.normal.normal_y * 0.5;
normal_end.z = mid_point.z + line.normal.normal_z * 0.5;
normal_marker.points.push_back(mid_point);
normal_marker.points.push_back(normal_end);
normal_markers.markers.push_back(normal_marker);
}
normals_pub.publish(normal_markers);
}
void publishNearestPointMarker(const geometry_msgs::Point& point,float distance,const std::string& frame_id, const std::string& ref_name) {
visualization_msgs::Marker marker;
marker.header.frame_id = frame_id;
marker.header.stamp = ros::Time::now();
marker.ns = "nearest_point" + ref_name; //
marker.id = 0;
marker.type = visualization_msgs::Marker::SPHERE;
marker.action = visualization_msgs::Marker::ADD;
marker.pose.position = point;
marker.pose.orientation.w = 1.0;
marker.scale.x = 0.15;
marker.scale.y = 0.15;
marker.scale.z = 0.15;
marker.color.r = 1.0;
marker.color.g = 0.0;
marker.color.b = 0.0;
marker.color.a = 1.0;
marker.lifetime = ros::Duration(0.1);
nearest_point_pub.publish(marker);
//std_msgs::Float64 dist;
//dist.data =(point.y < 0 ? -1.0 : 1.0) *(std::fabs(point.y) - 0.12);
//dist.data = std::fabs(point.y);
//nearest_distance_pub.publish(dist); //最近距离
std_msgs::Float64 dist_msg;
if (ref_name == "left_front")
{
dist_msg.data = distance;
left_front_nearest_distance_pub.publish(dist_msg);
}
else if (ref_name == "left_back")
{
dist_msg.data = distance;
left_back_nearest_distance_pub.publish(dist_msg);
}
else if (ref_name == "right_front")
{
dist_msg.data = distance;
right_front_nearest_distance_pub.publish(dist_msg);
}
else
{
dist_msg.data = distance;
right_back_nearest_distance_pub.publish(dist_msg);
} //
}
/***************************************************************/
void publishFootPointMarker(const geometry_msgs::Point& left_front_foot, //垂足标记
const geometry_msgs::Point& left_back_foot, //垂足标记
const geometry_msgs::Point& right_front_foot,
const geometry_msgs::Point& right_back_foot,
const std::string& frame_id) {
visualization_msgs::Marker marker;
marker.header.frame_id = frame_id;
marker.header.stamp = ros::Time::now();
marker.ns = "foot_points";
marker.id = 0;
marker.type = visualization_msgs::Marker::POINTS;
marker.action = visualization_msgs::Marker::ADD;
marker.scale.x = 0.1; // 点的大小
marker.scale.y = 0.1;
marker.color.a = 1.0; // 不透明度
// 添加左侧垂足点 - 蓝色
marker.points.push_back(left_front_foot);
std_msgs::ColorRGBA color;
color.r = 0.0;
color.g = 0.0;
color.b = 1.0;
color.a = 1.0;
marker.colors.push_back(color);
// 添加右侧垂足点 - 绿色
marker.points.push_back(right_front_foot);
color.r = 0.0;
color.g = 1.0;
color.b = 0.0;
color.a = 1.0;
marker.colors.push_back(color);
// 添加左侧垂足点 - 蓝色
marker.points.push_back(left_back_foot);
color.r = 0.0;
color.g = 1.0;
color.b = 1.0;
color.a = 1.0;
marker.colors.push_back(color);
// 添加右侧垂足点 - 绿色
marker.points.push_back(right_back_foot);
color.r = 1.0;
color.g = 1.0;
color.b = 0.0;
color.a = 1.0;
marker.colors.push_back(color);
foot_points_pub.publish(marker);
}
/*******************************************************************/
void publishTFPoints(const geometry_msgs::Point& left_front_point,
const geometry_msgs::Point& left_back_point,
const geometry_msgs::Point& right_front_point,
const geometry_msgs::Point& right_back_point,
const std::string& frame_id) {
visualization_msgs::Marker marker;
marker.header.frame_id = frame_id;
marker.header.stamp = ros::Time::now();
marker.ns = "tf_point";
marker.id = 0;
marker.type = visualization_msgs::Marker::POINTS;
marker.action = visualization_msgs::Marker::ADD;
marker.scale.x = 0.15;
marker.scale.y = 0.15;
marker.color.a = 1.0;
// 左侧点 - 蓝色
geometry_msgs::Point p;
p = left_front_point;
marker.points.push_back(p);
std_msgs::ColorRGBA c;
c.r = 0.0;
c.g = 0.0;
c.b = 1.0;
c.a = 1.0;
marker.colors.push_back(c);
// 右侧点 - 绿色
p = right_front_point;
marker.points.push_back(p);
c.r = 0.0;
c.g = 1.0;
c.b = 0.0;
c.a = 1.0;
marker.colors.push_back(c);
// 左侧点 - 蓝色
p = left_back_point;
marker.points.push_back(p);
c.r = 0.0;
c.g = 1.0;
c.b = 1.0;
c.a = 1.0;
marker.colors.push_back(c);
// 右侧点 - 绿色
p = right_back_point;
marker.points.push_back(p);
c.r = 1.0;
c.g = 1.0;
c.b = 0.0;
c.a = 1.0;
marker.colors.push_back(c);
tf_points_pub.publish(marker);
}
void publishDistanceInfo(const std::string& frame_id,
double left_front_dist,
double left_back_dist,
double right_front_dist,
double right_back_dist,
const geometry_msgs::Point& wall_point) {
visualization_msgs::Marker marker;
marker.header.frame_id = frame_id;
marker.header.stamp = ros::Time::now();
marker.ns = "distance_info";
marker.id = 0;
marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
marker.action = visualization_msgs::Marker::ADD;
marker.pose.position = wall_point;
marker.pose.position.z += 0.5; // 在墙上点上方显示
marker.pose.orientation.w = 1.0;
marker.scale.z = 0.2; // 文本大小
marker.color.r = 1.0;
marker.color.g = 1.0;
marker.color.b = 0.0;
marker.color.a = 1.0;
std::stringstream ss;
ss << std::fixed << std::setprecision(2);
ss << "Left_front_tf: " << left_front_dist << "m\n";
ss << "Left_back_tf: " << left_back_dist << "m\n";
ss << "Right_front_tf: " << right_front_dist << "m\n";
ss << "Right_back_tf: " << right_back_dist << "m\n";
if (left_front_dist < right_front_dist && left_back_dist < right_back_dist ) {
ss << "Left is closer";
} else if (right_front_dist < left_front_dist && right_back_dist < left_back_dist ) {
ss << "Right is closer";
} else {
ss << "Equal distance";
}
marker.text = ss.str();
min_distance_pub.publish(marker);
}
float min_four(float a ,float b , float c , float d )
{
return std::min({a, b, c, d});
}
void LidarCallback(const sensor_msgs::LaserScan::ConstPtr& msg)
{
PointCloudT::Ptr original_cloud(new PointCloudT);
std::unordered_map<int, PointData> point_data_map;
PointCloudT::Ptr cloud(new PointCloudT);
cloud->header.frame_id = msg->header.frame_id;
cloud->height = 1;
cloud->is_dense = false;
float min_distance = std::numeric_limits<float>::max();
int min_index = -1;
int point_count = 0;
const float min_angle1 = 30 * M_PI/180.0;
const float max_angle1 = 150* M_PI/180.0;
const float min_angle2 = -150 * M_PI/180.0;
const float max_angle2 = -30 * M_PI/180.0;
// 为参考点初始化最小距离
float left_front_min_distance = std::numeric_limits<float>::max();
int left_front_min_index = -1;
geometry_msgs::Point left_front_nearest_point;
float left_back_min_distance = std::numeric_limits<float>::max();
int left_back_min_index = -1;
geometry_msgs::Point left_back_nearest_point;
// 为参考点初始化最小距离
float right_front_min_distance = std::numeric_limits<float>::max();
int right_front_min_index = -1;
geometry_msgs::Point right_front_nearest_point;
float right_back_min_distance = std::numeric_limits<float>::max();
int right_back_min_index = -1;
geometry_msgs::Point right_back_nearest_point;
// 寻找最近点
for (size_t i = 0; i < msg->ranges.size(); ++i)
{
const float range = msg->ranges[i];
if (std::isnan(range)) continue;
if (range < msg->range_min || range > msg->range_max) continue;
const float angle = msg -> angle_min + i * msg -> angle_increment;
//创建屏蔽条件检测
bool in_blocked_zone = true;
float normalized_angle = angle;
const float x = range * cos(angle);
const float y = range * sin(angle);
if (choose_wall == "left")
{
// 左侧
if (angle >= min_angle1 && angle <= max_angle1)
{
if ( x > left_back_ref.x && x < left_front_ref.x)
{
in_blocked_zone= false;
}
}
}
else if (choose_wall == "right")
{
// 右侧
if (angle >= min_angle2 && angle <= max_angle2)
{
if ( x > right_back_ref.x && x < right_front_ref.x)
{
in_blocked_zone= false;
}
}
}
else
{
if (angle >= min_angle1 && angle <= max_angle1)
{
in_blocked_zone= false;
}
if (angle >= min_angle2 && angle <= max_angle2)
{
in_blocked_zone= false;
}
}
if (in_blocked_zone) continue;
if (range < min_distance)
{
min_distance = range;
min_index = i;
}
PointT point;
point.x = range * cos(angle);
point.y = range * sin(angle);
point.z = 0.0;
point.r = 0;
point.g = 255;
point.b = 0;
PointData data;
data.original_index = i;
data.is_line_point = false;
data.is_nearest = (i == min_index);
point_data_map[point_count] = data;
cloud->points.push_back(point);
original_cloud->points.push_back(point);
point_count++;
}
cloud->width = point_count;
// 如果点云为空,直接返回
if (cloud->empty()) {
ROS_WARN_THROTTLE(1.0, "No valid points found");
return;
}
std::vector<DetectedLine> detected_lines;
pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>);
tree->setInputCloud(cloud);
// 2. 执行欧几里得聚类 - 确保连续性
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<PointT> ec;
ec.setClusterTolerance(0.3); // 点间最大距离阈值(米)
ec.setMinClusterSize(15); // 最小聚类点数
ec.setMaxClusterSize(10000); // 最大聚类点数
ec.setSearchMethod(tree);
ec.setInputCloud(cloud);
ec.extract(cluster_indices);
ROS_INFO_THROTTLE(1.0, "Detected %zu point cloud clusters", cluster_indices.size());
std::vector<std::vector<uint8_t>> colors =
{
{255, 0, 0}, // 红
{0, 255, 0}, // 绿
{0, 0, 255}, // 蓝
{255, 255, 0}, // 黄
{0, 255, 255}, // 青
{255, 0, 255} // 紫
};
// 初始化点云颜色
for (auto& point : cloud->points)
{
point.r = 0;
point.g = 255;
point.b = 0;
}
// 3. 对每个聚类进行直线检测
for (size_t i = 0; i < cluster_indices.size(); i++)
{
const auto& cluster = cluster_indices[i];
// 创建当前聚类的点云
PointCloudT::Ptr cluster_cloud(new PointCloudT);
for (const auto& idx : cluster.indices)
{
cluster_cloud->push_back((*cloud)[idx]);
}
// 为当前聚类的点着色
const auto& color = colors[i % colors.size()];
for (const auto& idx : cluster.indices)
{
cloud->points[idx].r = color[0];
cloud->points[idx].g = color[1];
cloud->points[idx].b = color[2];
}
// 跳过点数过少的聚类
if (cluster_cloud->size() < 10) continue;
pcl::SACSegmentation<PointT> seg;
pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
seg.setOptimizeCoefficients(true);
seg.setModelType(pcl::SACMODEL_LINE);
seg.setMethodType(pcl::SAC_RANSAC);
seg.setMaxIterations(1000);
seg.setDistanceThreshold(0.1); // 点到直线的最大距离阈值
seg.setInputCloud(cluster_cloud);
seg.segment(*inliers, *coefficients);
if (inliers->indices.size() < 10) continue;
DetectedLine line;
float min_proj = std::numeric_limits<float>::max();
float max_proj = std::numeric_limits<float>::lowest();
Eigen::Vector3f direction(coefficients->values[3], coefficients->values[4], 0.0f);
direction.normalize();
// 计算直线的起点和终点
for (const auto& idx : inliers->indices)
{
Eigen::Vector3f pt(
cluster_cloud->points[idx].x,
cluster_cloud->points[idx].y,
0.0f
);
float proj = pt.dot(direction);
if (proj < min_proj)
{
min_proj = proj;
line.start.x = pt.x();
line.start.y = pt.y();
line.start.z = 0.0f;
}
if (proj > max_proj)
{
max_proj = proj;
line.end.x = pt.x();
line.end.y = pt.y();
line.end.z = 0.0f;
}
}
// 计算法线方向
line.normal.normal_x = -direction.y();
line.normal.normal_y = direction.x();
line.normal.normal_z = 0.0f;
line.direction = direction;
detected_lines.push_back(line);
// 标记直线点
for (const auto& inlier_idx : inliers->indices)
{
if (inlier_idx >= 0 && inlier_idx < cluster.indices.size())
{
int original_idx = cluster.indices[inlier_idx];
if (original_idx >= 0 && original_idx < cloud->size())
{
point_data_map[original_idx].is_line_point = true;
}
}
}
}
// 更新点云颜色
for (int i = 0; i < cloud->size(); i++)
{
if (point_data_map[i].is_line_point)
{
cloud->points[i].r = 255;
cloud->points[i].g = 255;
cloud->points[i].b = 0;
}
}
sensor_msgs::PointCloud2 cloud_msg;
pcl::toROSMsg(*cloud, cloud_msg);
cloud_msg.header = msg->header;
std::vector<DetectedLine> transformed_lines;
geometry_msgs::Point left_front_tf_point_map ,left_back_tf_point_map ;
geometry_msgs::Point right_front_tf_point_map , right_back_tf_point_map ;
bool left_front_tf_valid = false , left_back_tf_valid = false;
bool right_front_tf_valid = false , right_back_tf_valid = false;
if (tfBuffer) {
try {
if (!tfBuffer->canTransform("base_footprint", cloud_msg.header.frame_id, cloud_msg.header.stamp, ros::Duration(0.1))) {
ROS_WARN_THROTTLE(1.0, "TF transform not available");
return;
}
/*
// 转换点云
sensor_msgs::PointCloud2 transformed_cloud;
tfBuffer->transform(cloud_msg, transformed_cloud, "base_footprint");
transformed_cloud.header.stamp = ros::Time::now();
transformed_cloud.header.frame_id = "base_footprint";
pointcloud_pub.publish(transformed_cloud);
*/
// 转换检测到的直线
if (!detected_lines.empty()) {
for (auto& line : detected_lines) {
geometry_msgs::PointStamped laser_start, map_start;
laser_start.header = msg->header;
laser_start.point.x = line.start.x;
laser_start.point.y = line.start.y;
laser_start.point.z = 0.0f;
tfBuffer->transform(laser_start, map_start, "base_footprint");
geometry_msgs::PointStamped laser_end, map_end;
laser_end.header = msg->header;
laser_end.point.x = line.end.x;
laser_end.point.y = line.end.y;
laser_end.point.z = 0.0f;
tfBuffer->transform(laser_end, map_end, "base_footprint");
geometry_msgs::Vector3Stamped laser_normal, map_normal;
laser_normal.header = msg->header;
laser_normal.vector.x = line.normal.normal_x;
laser_normal.vector.y = line.normal.normal_y;
laser_normal.vector.z = 0.0f;
tfBuffer->transform(laser_normal, map_normal, "base_footprint");
DetectedLine transformed_line;
transformed_line.start.x = map_start.point.x;
transformed_line.start.y = map_start.point.y;
transformed_line.start.z = map_start.point.z;
transformed_line.end.x = map_end.point.x;
transformed_line.end.y = map_end.point.y;
transformed_line.end.z = map_end.point.z;
transformed_line.normal.normal_x = map_normal.vector.x;
transformed_line.normal.normal_y = map_normal.vector.y;
transformed_line.normal.normal_z = map_normal.vector.z;
transformed_lines.push_back(transformed_line);
}
publishDetectedLines(transformed_lines, "base_footprint");
}
// 获取TF参考点在base_footprint中的位置
try {
geometry_msgs::PointStamped left_front_tf_laser, left_front_tf_map;
left_front_tf_laser.header.frame_id = "laser_left_front";
left_front_tf_laser.header.stamp = ros::Time(0);
left_front_tf_laser.point.x = 0;
left_front_tf_laser.point.y = 0;
left_front_tf_laser.point.z = 0;
tfBuffer->transform(left_front_tf_laser, left_front_tf_map, "base_footprint");
left_front_tf_point_map = left_front_tf_map.point;
left_front_tf_valid = true;
geometry_msgs::PointStamped left_back_tf_laser, left_back_tf_map;
left_back_tf_laser.header.frame_id = "laser_left_back";
left_back_tf_laser.header.stamp = ros::Time(0);
left_back_tf_laser.point.x = 0;
left_back_tf_laser.point.y = 0;
left_back_tf_laser.point.z = 0;
tfBuffer->transform(left_back_tf_laser, left_back_tf_map, "base_footprint");
left_back_tf_point_map = left_back_tf_map.point;
left_back_tf_valid = true;
geometry_msgs::PointStamped right_front_tf_laser, right_front_tf_map;
right_front_tf_laser.header.frame_id = "laser_right_front";
right_front_tf_laser.header.stamp = ros::Time(0);
right_front_tf_laser.point.x = 0;
right_front_tf_laser.point.y = 0;
right_front_tf_laser.point.z = 0;
tfBuffer->transform(right_front_tf_laser, right_front_tf_map, "base_footprint");
right_front_tf_point_map = right_front_tf_map.point;
right_front_tf_valid = true;
geometry_msgs::PointStamped right_back_tf_laser, right_back_tf_map;
right_back_tf_laser.header.frame_id = "laser_right_back";
right_back_tf_laser.header.stamp = ros::Time(0);
right_back_tf_laser.point.x = 0;
right_back_tf_laser.point.y = 0;
right_back_tf_laser.point.z = 0;
tfBuffer->transform(right_back_tf_laser, right_back_tf_map, "base_footprint");
right_back_tf_point_map = right_back_tf_map.point;
right_back_tf_valid = true;
publishTFPoints(left_front_tf_point_map, left_back_tf_point_map,
right_front_tf_point_map, right_back_tf_point_map, "base_footprint");
} catch (tf2::TransformException& ex) {
ROS_WARN_THROTTLE(1.0, "TF point transform error: %s", ex.what());
}
} catch (tf2::TransformException& ex) {
ROS_WARN_THROTTLE(1.0, "TF point transform error: %s", ex.what());
}
}
if ( ! transformed_lines.empty() && left_front_tf_valid && left_back_tf_valid &&
right_front_tf_valid && right_back_tf_valid)
{
std::vector<DetectedLine>left_walls;
std::vector<DetectedLine>right_walls;
for (const auto& wall : transformed_lines)
{
if (isLeftWall(wall))
{
left_walls.push_back(wall);
}
else
{
right_walls.push_back(wall);
}
}
double left_front_min_dist = std::numeric_limits<double>::max();
double left_back_min_dist = std::numeric_limits<double>::max();
geometry_msgs::Point left_front_foot , left_back_foot;
geometry_msgs::Point left_front_wall_point, left_back_wall_point;
for (const auto& wall : left_walls)
{
geometry_msgs::Point start_point, end_point;
start_point.x = wall.start.x;
start_point.y = wall.start.y;
start_point.z = wall.start.z;
end_point.x = wall.end.x;
end_point.y = wall.end.y;
end_point.z = wall.end.z;
double dist_front = pointToLineDistance(
left_front_tf_point_map,
start_point,
end_point);
// 计算垂足点
geometry_msgs::Point foot_front = pointToLineProjection(
left_front_tf_point_map,
start_point,
end_point
);
if (dist_front < left_front_min_dist)
{
left_front_min_dist = dist_front;
left_front_foot = foot_front;
left_front_wall_point = foot_front;
}
double dist_back = pointToLineDistance(
left_back_tf_point_map,
start_point,
end_point);
// 计算垂足点
geometry_msgs::Point foot_back = pointToLineProjection(
left_back_tf_point_map,
start_point,
end_point
);
if (dist_back < left_back_min_dist) {
left_back_min_dist = dist_back;
left_back_foot = foot_back;
left_back_wall_point = foot_back;
}
}
//右墙
double right_front_min_dist = std::numeric_limits<double>::max();
double right_back_min_dist = std::numeric_limits<double>::max();
geometry_msgs::Point right_front_foot, right_back_foot;
geometry_msgs::Point right_front_wall_point, right_back_wall_point;
for (const auto& wall : right_walls)
{
geometry_msgs::Point start_point, end_point;
start_point.x = wall.start.x;
start_point.y = wall.start.y;
start_point.z = wall.start.z;
end_point.x = wall.end.x;
end_point.y = wall.end.y;
end_point.z = wall.end.z;
// 计算右前参考点
double dist_front = pointToLineDistance(
right_front_tf_point_map,
start_point,
end_point
);
// 计算垂足点
geometry_msgs::Point foot_front = pointToLineProjection(
right_front_tf_point_map,
start_point,
end_point
);
if (dist_front < right_front_min_dist) {
right_front_min_dist = dist_front;
right_front_foot = foot_front;
right_front_wall_point = foot_front;
}
// 计算右后参考点
double dist_back = pointToLineDistance(
right_back_tf_point_map,
start_point,
end_point
);
// 计算垂足点
geometry_msgs::Point foot_back = pointToLineProjection(
right_back_tf_point_map,
start_point,
end_point
);
if (dist_back < right_back_min_dist) {
right_back_min_dist = dist_back;
right_back_foot = foot_back;
right_back_wall_point = foot_back;
}
}
try
{
PointCloudT::Ptr display_cloud(new PointCloudT);
Eigen::Vector2f left_vec(left_front_foot.x - left_back_foot.x,
left_front_foot.y - left_back_foot.y);
float left_length = left_vec.norm();
if (left_length > 0) left_vec.normalize();
// 计算右侧向量
Eigen::Vector2f right_vec(right_front_foot.x - right_back_foot.x,
right_front_foot.y - right_back_foot.y);
float right_length = right_vec.norm();
if (right_length > 0) right_vec.normalize();
// 处理原始点云
for (size_t i = 0; i < original_cloud->size(); ++i) {
const auto& point = original_cloud->points[i];
// 转换到base_footprint坐标系
geometry_msgs::PointStamped point_laser, point_base;
point_laser.header.frame_id = msg->header.frame_id;
point_laser.header.stamp = ros::Time(0);
point_laser.point.x = point.x;
point_laser.point.y = point.y;
point_laser.point.z = point.z;
try {
tfBuffer->transform(point_laser, point_base, "base_footprint");
Eigen::Vector2f pt(point_base.point.x, point_base.point.y);
bool keep_point = false;
// 检查是否在左侧垂足范围内
if (left_length > 0) {
Eigen::Vector2f to_left_back( pt.x() - left_back_foot.x ,
pt.y() - left_back_foot.y );
float proj = to_left_back.dot(left_vec);
if (proj >= 0 && proj <= left_length) {
keep_point = true;
}
}
// 检查是否在右侧垂足范围内
if (!keep_point && right_length > 0) {
Eigen::Vector2f to_right_back(pt.x() - right_back_foot.x ,
pt.y() - right_back_foot.y );
float proj = to_right_back.dot(right_vec);
if (proj >= 0 && proj <= right_length) {
keep_point = true;
}
}
if (keep_point) {
// 保留原始激光坐标系的点
display_cloud->push_back(point);
}
} catch (tf2::TransformException& ex) {
ROS_WARN_THROTTLE(1.0, "TF transform error: %s", ex.what());
// 转换失败时保留点
display_cloud->push_back(point);
}
}
// 发布点云
sensor_msgs::PointCloud2 cloud_msg;
pcl::toROSMsg(*display_cloud, cloud_msg);
cloud_msg.header.stamp = ros::Time::now();
cloud_msg.header.frame_id = msg->header.frame_id;
pointcloud_pub.publish(cloud_msg);
}catch (tf2::TransformException& ex) {
ROS_WARN_THROTTLE(1.0, "TF point transform error: %s", ex.what());}
std_msgs::Float64 left_front_distance_msg, left_back_distance_msg , right_front_distance_msg , right_back_distance_msg;
// 左前距离
double left_front_vertical_dist = (left_front_min_dist == std::numeric_limits<double>::max()) ?
10.0 : left_front_min_dist;
left_front_distance_msg.data = left_front_vertical_dist;
//left_front_nearest_distance_pub.publish(left_front_distance_msg);
// 左后距离
double left_back_vertical_dist = (left_back_min_dist == std::numeric_limits<double>::max()) ?
10.0 : left_back_min_dist;
left_back_distance_msg.data = left_back_vertical_dist;
//left_back_nearest_distance_pub.publish(left_back_distance_msg);
// 右前距离
double right_front_vertical_dist = (right_front_min_dist == std::numeric_limits<double>::max()) ?
10.0 : right_front_min_dist;
right_front_distance_msg.data = right_front_vertical_dist;
//right_front_nearest_distance_pub.publish(right_front_distance_msg);
// 右后距离
double right_back_vertical_dist = (right_back_min_dist == std::numeric_limits<double>::max()) ?
10.0 : right_back_min_dist;
right_back_distance_msg.data = right_back_vertical_dist;
//right_back_nearest_distance_pub.publish(right_back_distance_msg);
// 发布垂足点
publishFootPointMarker(left_front_foot, left_back_foot,
right_front_foot, right_back_foot, "base_footprint");
// 发布距离信息
geometry_msgs::Point wall_mid_point;
wall_mid_point.x = (left_front_wall_point.x + right_front_wall_point.x) / 2;
wall_mid_point.y = (left_front_wall_point.y + right_front_wall_point.y) / 2;
wall_mid_point.z = 0;
float dist_min = min_four(left_front_vertical_dist,left_back_vertical_dist,right_front_vertical_dist,right_back_vertical_dist);
if ( dist_min == left_front_vertical_dist )
{
nearest_distance_pub.publish(left_front_distance_msg);
}
else if ( dist_min == left_back_vertical_dist )
{
nearest_distance_pub.publish(left_back_distance_msg);
}
else if ( dist_min == right_front_vertical_dist )
{
nearest_distance_pub.publish(right_front_distance_msg);
}
else
{
nearest_distance_pub.publish(right_back_distance_msg);
}
publishDistanceInfo("base_footprint",
left_front_min_distance, left_back_min_distance,
right_front_min_distance, right_back_min_distance,
wall_mid_point);
ROS_INFO_THROTTLE(1.0, "Left Front TF to wall: %.3f m, Left Back TF to wall: %.3f m , Right Front TF to wall: %.3f m, Right Bcak TF to wall: %.3f m",
left_front_vertical_dist , left_back_vertical_dist ,right_front_vertical_dist ,right_back_vertical_dist );
}
}
”这个代码运行起来,rviz上观察不到较远距离的点云数据但是tf坐标对墙面的垂直距离计算正确
最新发布