JYC/RC2025_r1
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

r1-7-12

Browse Source
This commit is contained in:
Robofish 2025-07-12 22:39:19 +08:00
parent 2f3e6caa8d
commit 500f6bd1d8
19 changed files with 1143 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

63
src/rc_lidar/caijian.py Normal file
View File

@ -0,0 +1,63 @@
#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import PointCloud2, PointField
import numpy as np
import struct
from sklearn.cluster import DBSCAN
class LidarFilterNode(Node):
def __init__(self):
super().__init__('caijian_node')
self.publisher_ = self.create_publisher(PointCloud2, '/livox/lidar_filtered', 10)
self.subscription = self.create_subscription(
PointCloud2,
'/livox/lidar/pointcloud',
self.filter_callback,
10)
self.get_logger().info('caijian_node started, numpy filtering z in [1.5,3]m, distance<=12m, remove isolated points')
def filter_callback(self, msg):
num_points = msg.width * msg.height
data = np.frombuffer(msg.data, dtype=np.uint8)
points = np.zeros((num_points, 4), dtype=np.float32) # x, y, z, intensity
for i in range(num_points):
offset = i * msg.point_step
x = struct.unpack_from('f', data, offset)[0]
y = struct.unpack_from('f', data, offset + 4)[0]
z = struct.unpack_from('f', data, offset + 8)[0]
intensity = struct.unpack_from('f', data, offset + 12)[0]
points[i] = [x, y, z, intensity]
z_mask = (points[:,2] >= 1.5) & (points[:,2] <= 3.0)
dist_mask = np.linalg.norm(points[:,:3], axis=1) <= 16.0
mask = z_mask & dist_mask
filtered_points = points[mask]
# 使用DBSCAN去除孤立点
if filtered_points.shape[0] > 0:
clustering = DBSCAN(eps=0.3, min_samples=5).fit(filtered_points[:,:3])
core_mask = clustering.labels_ != -1
filtered_points = filtered_points[core_mask]
fields = [
PointField(name='x', offset=0, datatype=PointField.FLOAT32, count=1),
PointField(name='y', offset=4, datatype=PointField.FLOAT32, count=1),
PointField(name='z', offset=8, datatype=PointField.FLOAT32, count=1),
PointField(name='intensity', offset=12, datatype=PointField.FLOAT32, count=1),
]
filtered_points_list = filtered_points.tolist()
import sensor_msgs_py.point_cloud2 as pc2
filtered_msg = pc2.create_cloud(msg.header, fields, filtered_points_list)
self.publisher_.publish(filtered_msg)
def main(args=None):
rclpy.init(args=args)
node = LidarFilterNode()
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()

90
src/rc_lidar/circlr.py Normal file
View File

@ -0,0 +1,90 @@
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import PointCloud2
from geometry_msgs.msg import PointStamped
from sensor_msgs_py import point_cloud2 as pc2
import numpy as np
from sklearn.cluster import DBSCAN
import time
def statistical_outlier_removal(points, k=20, std_ratio=2.0):
from sklearn.neighbors import NearestNeighbors
nbrs = NearestNeighbors(n_neighbors=k+1).fit(points)
distances, _ = nbrs.kneighbors(points)
mean_dist = np.mean(distances[:, 1:], axis=1)
threshold = np.mean(mean_dist) + std_ratio * np.std(mean_dist)
mask = mean_dist < threshold
return points[mask]
class HoopFinder(Node):
def __init__(self):
super().__init__('find_hoop')
self.sub = self.create_subscription(
PointCloud2,
'/livox/lidar_filtered',
self.callback,
10)
self.pub = self.create_publisher(PointStamped, '/hoop_position', 10)
self.buffer = []
self.start_time = None
self.hoop_history = []
def callback(self, msg):
# 采集0.4秒内的点云
if self.start_time is None:
self.start_time = time.time()
for p in pc2.read_points(msg, field_names=("x", "y", "z", "intensity"), skip_nans=True):
self.buffer.append([p[0], p[1], p[2], p[3]])
if time.time() - self.start_time < 0.4:
return
points = np.array(self.buffer)
self.buffer = []
self.start_time = None
# 高度滤波
filtered = points[(points[:,2] > 1.0) & (points[:,2] < 3.0)]
if len(filtered) == 0:
return
# 统计离群点滤波
filtered = statistical_outlier_removal(filtered[:,:3], k=20, std_ratio=2.0)
# DBSCAN聚类
clustering = DBSCAN(eps=0.3, min_samples=10).fit(filtered)
labels = clustering.labels_
unique_labels = set(labels)
hoop_pos = None
max_cluster_size = 0
for label in unique_labels:
if label == -1:
continue
cluster = filtered[labels == label]
if len(cluster) > max_cluster_size:
max_cluster_size = len(cluster)
hoop_pos = np.mean(cluster, axis=0)
# 均值滤波输出
if hoop_pos is not None:
self.hoop_history.append(hoop_pos)
if len(self.hoop_history) > 5:
self.hoop_history.pop(0)
smooth_pos = np.mean(self.hoop_history, axis=0)
pt = PointStamped()
pt.header = msg.header
pt.point.x = float(smooth_pos[0])
pt.point.y = float(smooth_pos[1])
pt.point.z = float(smooth_pos[2])
self.pub.publish(pt)
self.get_logger().info(f"Hoop position (smoothed): {smooth_pos}")
def main(args=None):
rclpy.init(args=args)
node = HoopFinder()
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()

59
src/rc_lidar/find.py Normal file
View File

@ -0,0 +1,59 @@
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import PointCloud2
from geometry_msgs.msg import PointStamped
from sensor_msgs_py import point_cloud2 as pc2
import numpy as np
from sklearn.cluster import DBSCAN
class HoopFinder(Node):
def __init__(self):
super().__init__('find_hoop')
self.sub = self.create_subscription(
PointCloud2,
'/livox/lidar',
self.callback,
10)
self.pub = self.create_publisher(PointStamped, '/hoop_position', 10)
def callback(self, msg):
points = []
for p in pc2.read_points(msg, field_names=("x", "y", "z", "intensity"), skip_nans=True):
points.append([p[0], p[1], p[2], p[3]])
points = np.array(points)
filtered = points[(points[:,2] > 1.0) & (points[:,2] < 3.0)]
if len(filtered) == 0:
return
clustering = DBSCAN(eps=0.3, min_samples=10).fit(filtered[:,:3])
labels = clustering.labels_
unique_labels = set(labels)
hoop_pos = None
max_cluster_size = 0
for label in unique_labels:
if label == -1:
continue
cluster = filtered[labels == label]
if len(cluster) > max_cluster_size:
max_cluster_size = len(cluster)
hoop_pos = np.mean(cluster[:,:3], axis=0)
if hoop_pos is not None:
pt = PointStamped()
pt.header = msg.header
pt.point.x = float(hoop_pos[0])
pt.point.y = float(hoop_pos[1])
pt.point.z = float(hoop_pos[2])
self.pub.publish(pt)
self.get_logger().info(f"Hoop position: {hoop_pos}")
def main(args=None):
rclpy.init(args=args)
node = HoopFinder()
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()

63
src/rc_lidar/fliter.py Normal file
View File

@ -0,0 +1,63 @@
#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import PointCloud2, PointField
import numpy as np
import struct
from sklearn.cluster import DBSCAN
class LidarFilterNode(Node):
def __init__(self):
super().__init__('caijian_node')
self.publisher_ = self.create_publisher(PointCloud2, '/livox/lidar_filtered', 10)
self.subscription = self.create_subscription(
PointCloud2,
'/livox/lidar/pointcloud',
self.filter_callback,
10)
self.get_logger().info('caijian_node started, numpy filtering z in [1.5,3]m, distance<=12m, remove isolated points')
def filter_callback(self, msg):
num_points = msg.width * msg.height
data = np.frombuffer(msg.data, dtype=np.uint8)
points = np.zeros((num_points, 4), dtype=np.float32) # x, y, z, intensity
for i in range(num_points):
offset = i * msg.point_step
x = struct.unpack_from('f', data, offset)[0]
y = struct.unpack_from('f', data, offset + 4)[0]
z = struct.unpack_from('f', data, offset + 8)[0]
intensity = struct.unpack_from('f', data, offset + 12)[0]
points[i] = [x, y, z, intensity]
z_mask = (points[:,2] >= 1.5) & (points[:,2] <= 3.0)
dist_mask = np.linalg.norm(points[:,:3], axis=1) <= 12.0
mask = z_mask & dist_mask
filtered_points = points[mask]
# 使用DBSCAN去除孤立点
if filtered_points.shape[0] > 0:
clustering = DBSCAN(eps=0.3, min_samples=5).fit(filtered_points[:,:3])
core_mask = clustering.labels_ != -1
filtered_points = filtered_points[core_mask]
fields = [
PointField(name='x', offset=0, datatype=PointField.FLOAT32, count=1),
PointField(name='y', offset=4, datatype=PointField.FLOAT32, count=1),
PointField(name='z', offset=8, datatype=PointField.FLOAT32, count=1),
PointField(name='intensity', offset=12, datatype=PointField.FLOAT32, count=1),
]
filtered_points_list = filtered_points.tolist()
import sensor_msgs_py.point_cloud2 as pc2
filtered_msg = pc2.create_cloud(msg.header, fields, filtered_points_list)
self.publisher_.publish(filtered_msg)
def main(args=None):
rclpy.init(args=args)
node = LidarFilterNode()
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()

340
src/rc_lidar/juxing.py Normal file
View File

@ -0,0 +1,340 @@
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import PointCloud2, PointField
import numpy as np
import struct
from sklearn.cluster import DBSCAN
import cv2
from visualization_msgs.msg import Marker
from sklearn.linear_model import RANSACRegressor
def ransac_line_3d(points, threshold=0.05, min_inliers=20):
best_inliers = []
best_line = None
N = len(points)
if N < min_inliers:
return None, []
for _ in range(100):
idx = np.random.choice(N, 2, replace=False)
p1, p2 = points[idx]
v = p2 - p1
v = v / np.linalg.norm(v)
dists = np.linalg.norm(np.cross(points - p1, v), axis=1)
inliers = np.where(dists < threshold)[0]
if len(inliers) > len(best_inliers):
best_inliers = inliers
best_line = (p1, p2)
if len(best_inliers) > N * 0.5:
break
return best_line, best_inliers
def fit_rectangle_pca(cluster):
# 用PCA找主方向和边界点
pts = cluster[:, :3]
mean = np.mean(pts, axis=0)
cov = np.cov(pts.T)
eigvals, eigvecs = np.linalg.eigh(cov)
order = np.argsort(eigvals)[::-1]
main_dir = eigvecs[:, order[0]]
second_dir = eigvecs[:, order[1]]
# 投影到主方向和次方向
proj_main = np.dot(pts - mean, main_dir)
proj_second = np.dot(pts - mean, second_dir)
# 找四个角点
corners = []
for xm in [np.min(proj_main), np.max(proj_main)]:
for xs in [np.min(proj_second), np.max(proj_second)]:
idx = np.argmin((proj_main - xm)**2 + (proj_second - xs)**2)
corners.append(pts[idx])
return corners
def rectangle_score(pts):
# 评估4个点是否接近矩形
d = [np.linalg.norm(pts[i] - pts[(i+1)%4]) for i in range(4)]
diag1 = np.linalg.norm(pts[0] - pts[2])
diag2 = np.linalg.norm(pts[1] - pts[3])
w = max(d)
h = min(d)
ratio = w / h if h > 0 else 0
ideal_ratio = 1.8 / 1.05
score = abs(ratio - ideal_ratio) + abs(diag1 - diag2) / max(diag1, diag2)
return score
def classify_lines(lines):
# lines: 每条线是 [x, y, z, intensity]
# 假设 lines 是8个端点，两两为一条线
vertical_lines = []
horizontal_lines = []
for i in range(0, len(lines), 2):
p1 = np.array(lines[i][:3])
p2 = np.array(lines[i+1][:3])
vec = p2 - p1
length = np.linalg.norm(vec)
# 计算与地面的夹角（假设地面为z轴为0，垂直为z方向）
dz = abs(vec[2])
dxy = np.linalg.norm(vec[:2])
# 垂直线：z方向分量大，长度约1.05m
if dz > dxy and 0.95 < length < 1.15:
vertical_lines.append((i, i+1, length))
# 水平线：z方向分量小，长度约1.8m
elif dz < dxy and 1.7 < length < 1.9:
horizontal_lines.append((i, i+1, length))
return vertical_lines, horizontal_lines
def find_best_rectangle_from_lines(lines):
vertical_lines, horizontal_lines = classify_lines(lines)
# 只选出2条垂直线和2条水平线
if len(vertical_lines) < 2 or len(horizontal_lines) < 2:
return None
# 取长度最接近目标的两条
vertical_lines = sorted(vertical_lines, key=lambda x: abs(x[2]-1.05))[:2]
horizontal_lines = sorted(horizontal_lines, key=lambda x: abs(x[2]-1.8))[:2]
# 组合4个端点
indices = [vertical_lines[0][0], vertical_lines[0][1],
vertical_lines[1][0], vertical_lines[1][1],
horizontal_lines[0][0], horizontal_lines[0][1],
horizontal_lines[1][0], horizontal_lines[1][1]]
# 去重，只保留4个顶点
unique_indices = list(set(indices))
if len(unique_indices) < 4:
return None
pts = [lines[idx] for idx in unique_indices[:4]]
# 按矩形评分排序
from itertools import permutations
best_score = float('inf')
best_rect = None
for order in permutations(range(4)):
ordered = [pts[i] for i in order]
score = rectangle_score(np.array([p[:3] for p in ordered]))
if score < best_score:
best_score = score
best_rect = ordered
return best_rect
class BasketballFrameDetector(Node):
def __init__(self):
super().__init__('basketball_frame_detector')
self.subscription = self.create_subscription(
PointCloud2,
'/livox/lidar_filtered',
self.pointcloud_callback,
10
)
self.publisher = self.create_publisher(
PointCloud2,
'/basketball_frame_cloud',
10
)
self.marker_pub = self.create_publisher(
Marker,
'/basketball_frame_lines',
10
)
self.pointcloud_buffer = []
self.max_buffer_size = 10 # 减少缓冲帧数，加快响应
self.center_buffer = []
self.center_buffer_size = 5
def pointcloud_callback(self, msg):
points = self.pointcloud2_to_xyz(msg)
if points.shape[0] == 0:
return # 跳过空点云
self.pointcloud_buffer.append(points)
# 只保留非空点云
self.pointcloud_buffer = [arr for arr in self.pointcloud_buffer if arr.shape[0] > 0]
if len(self.pointcloud_buffer) > self.max_buffer_size:
self.pointcloud_buffer.pop(0)
all_points = np.vstack(self.pointcloud_buffer)
xy_points = all_points[:, :2]
if len(xy_points) < 10:
self.get_logger().info('点数太少，跳过')
return
clustering = DBSCAN(eps=0.3, min_samples=10).fit(xy_points)
labels = clustering.labels_
unique_labels = set(labels)
found = False
for label in unique_labels:
if label == -1:
continue
cluster = all_points[labels == label]
if len(cluster) < 30:
continue
min_x, min_y = np.min(cluster[:, :2], axis=0)
max_x, max_y = np.max(cluster[:, :2], axis=0)
width = abs(max_x - min_x)
height = abs(max_y - min_y)
if 1.5 < width < 2.1 and 0.7 < height < 1.3:
cloud_msg = self.xyz_array_to_pointcloud2(cluster, msg.header)
self.publisher.publish(cloud_msg)
# 用PCA直接找矩形四角
corners = fit_rectangle_pca(cluster)
from itertools import permutations
best_score = float('inf')
best_rect = None
for order in permutations(range(4)):
ordered = [corners[i] for i in order]
score = rectangle_score(np.array(ordered))
if score < best_score:
best_score = score
best_rect = ordered
rect_lines = best_rect
if rect_lines is None or len(rect_lines) < 4:
self.get_logger().info('未找到合适矩形')
continue
# 发布最新的矩形
marker = Marker()
marker.header = msg.header
marker.ns = "basketball_frame"
marker.id = 0
marker.type = Marker.LINE_LIST
marker.action = Marker.ADD
marker.scale.x = 0.08
marker.color.r = 0.0
marker.color.g = 1.0
marker.color.b = 0.0
marker.color.a = 1.0
marker.points = []
from geometry_msgs.msg import Point
for i in range(4):
p1 = rect_lines[i]
p2 = rect_lines[(i+1)%4]
pt1 = Point(x=float(p1[0]), y=float(p1[1]), z=float(p1[2]))
pt2 = Point(x=float(p2[0]), y=float(p2[1]), z=float(p2[2]))
marker.points.append(pt1)
marker.points.append(pt2)
self.marker_pub.publish(marker)
# 分别发布4条最优边线
for i in range(4):
edge_marker = Marker()
edge_marker.header = msg.header
edge_marker.ns = "basketball_frame_edges"
edge_marker.id = i
edge_marker.type = Marker.LINE_STRIP
edge_marker.action = Marker.ADD
edge_marker.scale.x = 0.12
colors = [
(1.0, 0.0, 0.0),
(0.0, 1.0, 0.0),
(0.0, 0.0, 1.0),
(1.0, 1.0, 0.0)
]
edge_marker.color.r = colors[i][0]
edge_marker.color.g = colors[i][1]
edge_marker.color.b = colors[i][2]
edge_marker.color.a = 1.0
pt1 = Point(x=float(rect_lines[i][0]), y=float(rect_lines[i][1]), z=float(rect_lines[i][2]))
pt2 = Point(x=float(rect_lines[(i+1)%4][0]), y=float(rect_lines[(i+1)%4][1]), z=float(rect_lines[(i+1)%4][2]))
edge_marker.points = [pt1, pt2]
self.marker_pub.publish(edge_marker)
# 中心点滑动平均
center = np.mean(np.array(rect_lines), axis=0)
self.center_buffer.append(center)
if len(self.center_buffer) > self.center_buffer_size:
self.center_buffer.pop(0)
stable_center = np.mean(self.center_buffer, axis=0)
# 发布中心点Marker
center_marker = Marker()
center_marker.header = msg.header
center_marker.ns = "basketball_frame"
center_marker.id = 1
center_marker.type = Marker.SPHERE
center_marker.action = Marker.ADD
center_marker.scale.x = 0.15
center_marker.scale.y = 0.15
center_marker.scale.z = 0.15
center_marker.color.r = 1.0
center_marker.color.g = 0.0
center_marker.color.b = 0.0
center_marker.color.a = 1.0
center_marker.pose.position.x = float(stable_center[0])
center_marker.pose.position.y = float(stable_center[1])
center_marker.pose.position.z = float(stable_center[2])
center_marker.pose.orientation.x = 0.0
center_marker.pose.orientation.y = 0.0
center_marker.pose.orientation.z = 0.0
center_marker.pose.orientation.w = 1.0
self.marker_pub.publish(center_marker)
# 计算法向量（篮板主方向，PCA最小特征值方向）
pts = np.array(rect_lines)
mean = np.mean(pts, axis=0)
cov = np.cov(pts.T)
eigvals, eigvecs = np.linalg.eigh(cov)
order = np.argsort(eigvals)[::-1]
normal_vec = eigvecs[:, order[2]]
normal_vec = normal_vec / np.linalg.norm(normal_vec)
# 向内侧偏移30cm
offset_point = stable_center + 0.3 * normal_vec
# 发布偏移点Marker
offset_marker = Marker()
offset_marker.header = msg.header
offset_marker.ns = "basketball_frame"
offset_marker.id = 2
offset_marker.type = Marker.SPHERE
offset_marker.action = Marker.ADD
offset_marker.scale.x = 0.12
offset_marker.scale.y = 0.12
offset_marker.scale.z = 0.12
offset_marker.color.r = 0.0
offset_marker.color.g = 0.0
offset_marker.color.b = 1.0
offset_marker.color.a = 1.0
offset_marker.pose.position.x = float(offset_point[0])
offset_marker.pose.position.y = float(offset_point[1])
offset_marker.pose.position.z = float(offset_point[2])
offset_marker.pose.orientation.x = 0.0
offset_marker.pose.orientation.y = 0.0
offset_marker.pose.orientation.z = 0.0
offset_marker.pose.orientation.w = 1.0
self.marker_pub.publish(offset_marker)
found = True
break
if not found:
self.get_logger().info('本帧未找到矩形')
def pointcloud2_to_xyz(self, cloud_msg):
fmt = 'ffff'
points = []
for i in range(cloud_msg.width):
offset = i * cloud_msg.point_step
x, y, z, intensity = struct.unpack_from(fmt, cloud_msg.data, offset)
points.append([x, y, z, intensity])
return np.array(points)
def xyz_array_to_pointcloud2(self, points, header):
msg = PointCloud2()
msg.header = header
msg.height = 1
msg.width = len(points)
msg.is_dense = True
msg.is_bigendian = False
msg.point_step = 16
msg.row_step = msg.point_step * msg.width
msg.fields = [
PointField(name='x', offset=0, datatype=7, count=1),
PointField(name='y', offset=4, datatype=7, count=1),
PointField(name='z', offset=8, datatype=7, count=1),
PointField(name='intensity', offset=12, datatype=7, count=1),
]
msg.data = b''.join([struct.pack('ffff', *p) for p in points])
return msg
def main(args=None):
rclpy.init(args=args)
node = BasketballFrameDetector()
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()

0
src/rc_lidar/line.py Normal file
View File

75
src/rc_lidar/pcd2pgm.py Normal file
View File

@ -0,0 +1,75 @@
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import PointCloud2
from nav_msgs.msg import OccupancyGrid
import numpy as np
import struct
import time
class PointCloudToGrid(Node):
def __init__(self):
super().__init__('pointcloud_to_grid')
self.subscription = self.create_subscription(
PointCloud2,
'/livox/lidar_filtered',
self.pointcloud_callback,
10)
self.publisher = self.create_publisher(OccupancyGrid, '/lidar_grid', 10)
self.grid_size = 2000
self.resolution = 0.02
self.origin_x = -20.0
self.origin_y = -20.0
self.points_buffer = []
self.last_header = None
# 定时器每0.5秒触发一次
self.timer = self.create_timer(0.5, self.publish_grid)
def pointcloud_callback(self, msg):
points = self.pointcloud2_to_xyz_array(msg)
self.points_buffer.append(points)
self.last_header = msg.header # 保存最新header用于地图消息
def publish_grid(self):
if not self.points_buffer:
return
# 合并0.5秒内所有点
all_points = np.concatenate(self.points_buffer, axis=0)
grid = np.zeros((self.grid_size, self.grid_size), dtype=np.int8)
for x, y, z in all_points:
if z < 2.0:
ix = int((x - self.origin_x) / self.resolution)
iy = int((y - self.origin_y) / self.resolution)
if 0 <= ix < self.grid_size and 0 <= iy < self.grid_size:
grid[iy, ix] = 100
grid_msg = OccupancyGrid()
if self.last_header:
grid_msg.header = self.last_header
grid_msg.info.resolution = self.resolution
grid_msg.info.width = self.grid_size
grid_msg.info.height = self.grid_size
grid_msg.info.origin.position.x = self.origin_x
grid_msg.info.origin.position.y = self.origin_y
grid_msg.data = grid.flatten().tolist()
self.publisher.publish(grid_msg)
self.points_buffer.clear() # 清空缓存
def pointcloud2_to_xyz_array(self, cloud_msg):
# 解析 PointCloud2 数据为 numpy 数组
fmt = 'fff' # x, y, z
point_step = cloud_msg.point_step
data = cloud_msg.data
points = []
for i in range(0, len(data), point_step):
x, y, z = struct.unpack_from(fmt, data, i)
points.append([x, y, z])
return np.array(points)
def main(args=None):
rclpy.init(args=args)
node = PointCloudToGrid()
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()

55
src/rc_lidar/save_pcd.py Normal file
View File

@ -0,0 +1,55 @@
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import PointCloud2
import numpy as np
import open3d as o3d
import time
class PointCloudSaver(Node):
def __init__(self):
super().__init__('pcd_saver')
self.subscription = self.create_subscription(
PointCloud2,
'/livox/lidar_filtered',
self.listener_callback,
10)
self.point_clouds = []
self.start_time = time.time()
self.timer = self.create_timer(3.0, self.save_and_exit)
self.saving = False
def listener_callback(self, msg):
if not self.saving:
pc = self.pointcloud2_to_xyz_array(msg)
if pc is not None:
self.point_clouds.append(pc)
def pointcloud2_to_xyz_array(self, cloud_msg):
# 仅支持 x, y, z 均为 float32 且无 padding 的点云
import struct
points = []
point_step = cloud_msg.point_step
for i in range(cloud_msg.width * cloud_msg.height):
offset = i * point_step
x, y, z = struct.unpack_from('fff', cloud_msg.data, offset)
points.append([x, y, z])
return np.array(points)
def save_and_exit(self):
if not self.saving:
self.saving = True
if self.point_clouds:
all_points = np.vstack(self.point_clouds)
pcd = o3d.geometry.PointCloud()
pcd.points = o3d.utility.Vector3dVector(all_points)
o3d.io.write_point_cloud("output.pcd", pcd)
self.get_logger().info("Saved output.pcd")
rclpy.shutdown()
def main(args=None):
rclpy.init(args=args)
saver = PointCloudSaver()
rclpy.spin(saver)
if __name__ == '__main__':
main()

102
src/rc_lidar/simple.py Normal file
View File

@ -0,0 +1,102 @@
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import PointCloud2, PointField
import numpy as np
import struct
from sklearn.cluster import DBSCAN
class BasketballFrameDetector(Node):
def __init__(self):
super().__init__('basketball_frame_detector')
self.subscription = self.create_subscription(
PointCloud2,
'/livox/lidar_filtered',
self.pointcloud_callback,
10
)
self.publisher = self.create_publisher(
PointCloud2,
'/basketball_frame_cloud',
10
)
self.pointcloud_buffer = []
def pointcloud_callback(self, msg):
points = self.pointcloud2_to_xyz(msg)
self.pointcloud_buffer.append(points)
self.get_logger().info(f'已保存点云组数: {len(self.pointcloud_buffer)}')
if len(self.pointcloud_buffer) < 10:
return
# 合并10组点云
all_points = np.vstack(self.pointcloud_buffer)
xy_points = all_points[:, :2]
self.get_logger().info(f'合并后点数: {xy_points.shape[0]}')
# 清空缓存，准备下一批
self.pointcloud_buffer = []
# 聚类识别
if len(xy_points) < 10:
return
clustering = DBSCAN(eps=0.3, min_samples=10).fit(xy_points)
labels = clustering.labels_
unique_labels = set(labels)
for label in unique_labels:
if label == -1:
continue
cluster = all_points[labels == label]
if len(cluster) < 30:
continue
min_x, min_y = np.min(cluster[:, :2], axis=0)
max_x, max_y = np.max(cluster[:, :2], axis=0)
width = abs(max_x - min_x)
height = abs(max_y - min_y)
self.get_logger().info(
f'聚类: label={label}, width={width:.2f}, height={height:.2f}, 点数={len(cluster)}'
)
if 1.6 < width < 2.0 and 0.8 < height < 1.2:
self.get_logger().info(
f'可能是篮球框: label={label}, width={width:.2f}, height={height:.2f}, 点数={len(cluster)}'
)
# 发布识别到的篮球框点云
cloud_msg = self.xyz_array_to_pointcloud2(cluster, msg.header)
self.publisher.publish(cloud_msg)
def pointcloud2_to_xyz(self, cloud_msg):
fmt = 'ffff' # x, y, z, intensity
points = []
for i in range(cloud_msg.width):
offset = i * cloud_msg.point_step
x, y, z, intensity = struct.unpack_from(fmt, cloud_msg.data, offset)
points.append([x, y, z, intensity])
return np.array(points)
def xyz_array_to_pointcloud2(self, points, header):
# 构造 PointCloud2 消息
msg = PointCloud2()
msg.header = header
msg.height = 1
msg.width = len(points)
msg.is_dense = True
msg.is_bigendian = False
msg.point_step = 16
msg.row_step = msg.point_step * msg.width
msg.fields = [
PointField(name='x', offset=0, datatype=7, count=1),
PointField(name='y', offset=4, datatype=7, count=1),
PointField(name='z', offset=8, datatype=7, count=1),
PointField(name='intensity', offset=12, datatype=7, count=1),
]
msg.data = b''.join([struct.pack('ffff', *p) for p in points])
return msg
def main(args=None):
rclpy.init(args=args)
node = BasketballFrameDetector()
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()

126
src/rc_lidar/simple_icp.py Normal file
View File

@ -0,0 +1,126 @@
#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import PointCloud2
from sensor_msgs_py import point_cloud2
from geometry_msgs.msg import PoseStamped
import open3d as o3d
import numpy as np
from transforms3d.quaternions import mat2quat
class PointCloudLocalization(Node):
def __init__(self):
super().__init__('point_cloud_localizer')
# 加载参考点云地图 (PCD文件)
self.reference_map = o3d.io.read_point_cloud("/home/robofish/RC2025/lankuang.pcd") # 替换为你的PCD文件路径
if not self.reference_map.has_points():
self.get_logger().error("Failed to load reference map!")
rclpy.shutdown()
# 预处理参考地图
self.reference_map = self.reference_map.voxel_down_sample(voxel_size=0.05)
self.reference_map.remove_statistical_outlier(nb_neighbors=20, std_ratio=2.0)[0]
# 创建ICP对象
self.icp = o3d.pipelines.registration.registration_icp
self.threshold = 0.5 # 匹配距离阈值 (米)
self.trans_init = np.identity(4) # 初始变换矩阵
# 订阅激光雷达点云
self.subscription = self.create_subscription(
PointCloud2,
'/livox/lidar_filtered',
self.lidar_callback,
10)
# 发布估计位置
self.pose_pub = self.create_publisher(PoseStamped, '/estimated_pose', 10)
self.get_logger().info("Point Cloud Localization Node Initialized!")
def ros_pc2_to_o3d(self, ros_cloud):
"""将ROS PointCloud2转换为Open3D点云"""
# 提取xyz坐标
points = point_cloud2.read_points(ros_cloud, field_names=("x", "y", "z"), skip_nans=True)
xyz = np.array([ [p[0], p[1], p[2]] for p in points ], dtype=np.float32)
if xyz.shape[0] == 0:
return None
# 创建Open3D点云
pcd = o3d.geometry.PointCloud()
pcd.points = o3d.utility.Vector3dVector(xyz)
return pcd
def preprocess_pointcloud(self, pcd):
"""点云预处理"""
# 降采样
pcd = pcd.voxel_down_sample(voxel_size=0.03)
# 移除离群点
pcd, _ = pcd.remove_statistical_outlier(nb_neighbors=20, std_ratio=1.0)
# 移除地面 (可选)
# plane_model, inliers = pcd.segment_plane(distance_threshold=0.1, ransac_n=3, num_iterations=100)
# pcd = pcd.select_by_index(inliers, invert=True)
return pcd
def lidar_callback(self, msg):
"""处理新的激光雷达数据"""
# 转换为Open3D格式
current_pcd = self.ros_pc2_to_o3d(msg)
if current_pcd is None:
self.get_logger().warn("Received empty point cloud!")
return
# 预处理当前点云
current_pcd = self.preprocess_pointcloud(current_pcd)
# 执行ICP配准
reg_result = self.icp(
current_pcd, self.reference_map, self.threshold,
self.trans_init,
o3d.pipelines.registration.TransformationEstimationPointToPoint(),
o3d.pipelines.registration.ICPConvergenceCriteria(max_iteration=50)
)
# 更新变换矩阵
self.trans_init = reg_result.transformation
# 提取位置和方向
translation = reg_result.transformation[:3, 3]
rotation_matrix = reg_result.transformation[:3, :3]
# 转换为四元数
quaternion = mat2quat(reg_result.transformation[:3, :3]) # 注意返回顺序为 [w, x, y, z]
# 发布位姿
pose_msg = PoseStamped()
pose_msg.header.stamp = self.get_clock().now().to_msg()
pose_msg.header.frame_id = "livox_frame"
pose_msg.pose.position.x = translation[0]
pose_msg.pose.position.y = translation[1]
pose_msg.pose.position.z = translation[2]
pose_msg.pose.orientation.x = quaternion[1] # x
pose_msg.pose.orientation.y = quaternion[2] # y
pose_msg.pose.orientation.z = quaternion[3] # z
pose_msg.pose.orientation.w = quaternion[0] # w
self.pose_pub.publish(pose_msg)
self.get_logger().info(f"Estimated Position: x={translation[0]:.2f}, y={translation[1]:.2f}, z={translation[2]:.2f}")
def main(args=None):
rclpy.init(args=args)
node = PointCloudLocalization()
try:
rclpy.spin(node)
except KeyboardInterrupt:
pass
finally:
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()

75
src/rc_lidar/xiamian.py Normal file
View File

@ -0,0 +1,75 @@
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import PointCloud2
from nav_msgs.msg import OccupancyGrid
import numpy as np
import struct
import time
class PointCloudToGrid(Node):
def __init__(self):
super().__init__('pointcloud_to_grid')
self.subscription = self.create_subscription(
PointCloud2,
'/livox/lidar_filtered',
self.pointcloud_callback,
10)
self.publisher = self.create_publisher(OccupancyGrid, '/lidar_grid', 10)
self.grid_size = 2000
self.resolution = 0.02
self.origin_x = -20.0
self.origin_y = -20.0
self.points_buffer = []
self.last_header = None
# 定时器每0.5秒触发一次
self.timer = self.create_timer(0.5, self.publish_grid)
def pointcloud_callback(self, msg):
points = self.pointcloud2_to_xyz_array(msg)
self.points_buffer.append(points)
self.last_header = msg.header # 保存最新header用于地图消息
def publish_grid(self):
if not self.points_buffer:
return
# 合并0.5秒内所有点
all_points = np.concatenate(self.points_buffer, axis=0)
grid = np.zeros((self.grid_size, self.grid_size), dtype=np.int8)
for x, y, z in all_points:
if z < 2.0:
ix = int((x - self.origin_x) / self.resolution)
iy = int((y - self.origin_y) / self.resolution)
if 0 <= ix < self.grid_size and 0 <= iy < self.grid_size:
grid[iy, ix] = 100
grid_msg = OccupancyGrid()
if self.last_header:
grid_msg.header = self.last_header
grid_msg.info.resolution = self.resolution
grid_msg.info.width = self.grid_size
grid_msg.info.height = self.grid_size
grid_msg.info.origin.position.x = self.origin_x
grid_msg.info.origin.position.y = self.origin_y
grid_msg.data = grid.flatten().tolist()
self.publisher.publish(grid_msg)
self.points_buffer.clear() # 清空缓存
def pointcloud2_to_xyz_array(self, cloud_msg):
# 解析 PointCloud2 数据为 numpy 数组
fmt = 'fff' # x, y, z
point_step = cloud_msg.point_step
data = cloud_msg.data
points = []
for i in range(0, len(data), point_step):
x, y, z = struct.unpack_from(fmt, data, i)
points.append([x, y, z])
return np.array(points)
def main(args=None):
rclpy.init(args=args)
node = PointCloudToGrid()
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()

51
src/rc_lidar/zhaoban.py Normal file
View File

@ -0,0 +1,51 @@
import numpy as np
from sklearn.cluster import DBSCAN
from scipy.optimize import leastsq
import matplotlib.pyplot as plt
def fit_circle(x, y):
# 拟合圆的函数
def calc_R(xc, yc):
return np.sqrt((x - xc)**2 + (y - yc)**2)
def f(c):
Ri = calc_R(*c)
return Ri - Ri.mean()
center_estimate = np.mean(x), np.mean(y)
center, _ = leastsq(f, center_estimate)
radius = calc_R(*center).mean()
return center[0], center[1], radius
def find_circle(points, eps=0.5, min_samples=10):
# 聚类
clustering = DBSCAN(eps=eps, min_samples=min_samples).fit(points)
labels = clustering.labels_
# 只取最大簇
unique, counts = np.unique(labels[labels != -1], return_counts=True)
if len(unique) == 0:
raise ValueError("未找到有效聚类")
main_cluster = unique[np.argmax(counts)]
cluster_points = points[labels == main_cluster]
x, y = cluster_points[:, 0], cluster_points[:, 1]
# 拟合圆
xc, yc, r = fit_circle(x, y)
return xc, yc, r, cluster_points
if __name__ == "__main__":
# 示例数据
np.random.seed(0)
angle = np.linspace(0, 2 * np.pi, 100)
x = 5 + 3 * np.cos(angle) + np.random.normal(0, 0.1, 100)
y = 2 + 3 * np.sin(angle) + np.random.normal(0, 0.1, 100)
points = np.vstack((x, y)).T
xc, yc, r, cluster_points = find_circle(points)
print(f"圆心: ({xc:.2f}, {yc:.2f}), 半径: {r:.2f}")
# 可视化
plt.scatter(points[:, 0], points[:, 1], label='所有点')
plt.scatter(cluster_points[:, 0], cluster_points[:, 1], label='聚类点')
circle = plt.Circle((xc, yc), r, color='r', fill=False, label='拟合圆')
plt.gca().add_patch(circle)
plt.legend()
plt.axis('equal')
plt.show()

0
src/rc_lidar/zhaoyuan.py Normal file
View File

2
src/rm_driver/rm_serial_driver/script/R2_Serial.py
View File

@ -18,6 +18,8 @@ class R2SerialNode(Node):
self.distance_port = self.get_parameter('distance_port').get_parameter_value().string_value
self.baud_rate = self.get_parameter('baud_rate').get_parameter_value().integer_value
# 创建定时器
# self.timer = self.create_timer(0.01, self.send_data) # 每100ms发送一次数据
# 初始化串口
try:
self.yaw_serial = serial.Serial(self.yaw_port, self.baud_rate, timeout=1)

36
src/rm_driver/rm_serial_driver/script/pub_goal.py Executable file
View File

@ -0,0 +1,36 @@
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import PointStamped
from rm_msgs.msg import MoveGoal
class ClickedGoalPublisher(Node):
def __init__(self):
super().__init__('clicked_goal_publisher')
self.subscription = self.create_subscription(
PointStamped,
'/clicked_point',
self.clicked_callback,
10
)
self.publisher = self.create_publisher(MoveGoal, '/move_goal', 10)
def clicked_callback(self, msg):
goal = MoveGoal()
goal.x = msg.point.x
goal.y = msg.point.y
goal.angle = 0.0
goal.max_speed = 0.0
goal.tolerance = 0.1
goal.rotor = False
self.publisher.publish(goal)
self.get_logger().info(f'发布目标: x={goal.x:.2f}, y={goal.y:.2f}')
def main(args=None):
rclpy.init(args=args)
node = ClickedGoalPublisher()
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()

2
src/rm_driver/rm_serial_driver/script/receive.py
View File

@ -36,7 +36,7 @@ class SerialReceiver(Node):
self.get_logger().info(f"打开串口 {SERIAL_PORT}，波特率 {BAUDRATE}")
# 创建定时器用于读取串口数据
self.timer = self.create_timer(0.01, self.read_serial_data) # 10ms
self.timer = self.create_timer(0.001, self.read_serial_data) # 10ms
self.buffer = bytearray()

6
src/rm_driver/rm_serial_driver/script/slect.py
View File

File diff suppressed because one or more lines are too long

2
src/rm_localization/icp_registration/include/icp_registration/icp_registration.hpp
View File

@ -50,7 +50,7 @@ private:
initial_pose_sub_;
rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr
pointcloud_sub_;
// rclcpp::TimerBase::SharedPtr timer_;
rclcpp::TimerBase::SharedPtr timer_;
std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
std::shared_ptr<tf2_ros::TransformListener> tf_listener_;

2
src/rm_nav_bringup/config/reality/measurement_params_real.yaml
View File

@ -1,3 +1,3 @@
base_link2livox_frame:
xyz: "\"0.246 -0.135 0.397\""
xyz: "\"0.251 -0.1285 0.397\""
rpy: "\"0.0 0.0 0.0\""