Havoc420mac/mi-task
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Refactor calibration logic in Robot_Ctrl class by uncommenting and optimizing the calibration process for odometry data. Ensure proper handling of calibration offset and improve clarity in the msg_handler_o method.

Browse Source
This commit is contained in:
havoc420ubuntu 2025-05-31 12:47:36 +00:00
parent 3b2fc33bf7
commit e427be6cb9
1 changed files with 10 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
main.py
View File

@ -126,7 +126,6 @@ class Robot_Ctrl(object):
self.odo_thread.start()
self.image_processor.run()
# self.marker_runner.run()
# self.marker_runner.run()
def msg_handler(self, channel, data):
self.rec_msg = robot_control_response_lcmt().decode(data)
@ -138,16 +137,16 @@ class Robot_Ctrl(object):
def msg_handler_o(self, channel, data):
self.odo_msg = localization_lcmt().decode(data)
# 如果尚未校准，记录第一帧数据作为校准基准
# if not self.is_calibrated:
# self.calibration_offset = self.odo_msg.xyz
# self.is_calibrated = True
# print(f"校准完成，基准值: {self.calibration_offset}")
# # 将接收到的 odo 数据减去校准基准值
# self.odo_msg.xyz = [
# self.odo_msg.xyz[0] - self.calibration_offset[0],
# self.odo_msg.xyz[1] - self.calibration_offset[1],
# self.odo_msg.xyz[2] - self.calibration_offset[2]
# ]
if not self.is_calibrated:
self.calibration_offset = self.odo_msg.xyz
self.is_calibrated = True
print(f"校准完成，基准值: {self.calibration_offset}")
# 将接收到的 odo 数据减去校准基准值
self.odo_msg.xyz = [
self.odo_msg.xyz[0] - self.calibration_offset[0],
self.odo_msg.xyz[1] - self.calibration_offset[1],
self.odo_msg.xyz[2] - self.calibration_offset[2]
]
# if self.odo_msg.timestamp % 100 == 0:
# print(self.odo_msg.xyz, self.odo_msg.rpy, self.odo_msg.vxyz, self.odo_msg.omegaBody, self.odo_msg.vBody)