icp算法实操 - Hello World

实操全过程

确保你已经在ROS环境中，可以通过运行以下命令来检查：
```
roscore
```
如果roscore成功启动，说明你已经在ROS环境中。
创建一个ROS包（如果尚未创建）：在终端中导航到你的ROS工作空间（通常是catkin_ws/src），然后使用catkin_create_pkg命令创建一个ROS包，例如：
```
cd ~/catkin_ws/src
catkin_create_pkg my_icp_save rospy sensor_msgs
```
这将创建一个名为my_icp_save的ROS包，并将rospy和sensor_msgs添加为依赖项。
编写你的Python节点：在my_ros_package包的目录下，创建一个Python脚本，例如my_node.py，并将你的Python节点代码放入其中。

#!/usr/bin/env python
import rospy
import scipy.io as sio
from sensor_msgs.msg import LaserScan

class LaserDataCollector:
    def __init__(self):
        rospy.init_node('laser_data_collector')
        self.front_scan_sub = rospy.Subscriber('/front_laser_scan_topic', LaserScan, self.front_scan_callback)
        self.rear_scan_sub = rospy.Subscriber('/rear_laser_scan_topic', LaserScan, self.rear_scan_callback)
        self.front_scan_data = None
        self.rear_scan_data = None

    def front_scan_callback(self, data):
        self.front_scan_data = {
            'angles': data.angle_min + data.angle_increment * range(len(data.ranges)),
            'ranges': data.ranges,
            'angle_increment': data.angle_increment
        }
        self.save_to_mat()

    def rear_scan_callback(self, data):
        self.rear_scan_data = {
            'angles': data.angle_min + data.angle_increment * range(len(data.ranges)),
            'ranges': data.ranges,
            'angle_increment': data.angle_increment
        }
        self.save_to_mat()

    def save_to_mat(self):
        if self.front_scan_data is not None and self.rear_scan_data is not None:
            # Combine and save the data as a MAT file
            combined_data = {
                'front_scan_data': self.front_scan_data,
                'rear_scan_data': self.rear_scan_data
            }
            sio.savemat('laser_scan_data.mat', combined_data)
            rospy.loginfo('Laser scan data saved to laser_scan_data.mat')

if __name__ == '__main__':
    try:
        data_collector = LaserDataCollector()
        rospy.spin()
    except rospy.ROSInterruptException:
        pass

构建ROS包：在终端中，返回到ROS工作空间的根目录，并运行catkin_make来构建你的ROS包：

cd ~/catkin_ws
catkin_make

文件位置和权限：确保你的my_save.py文件位于ROS包的src目录下，并且具有可执行权限。你可以使用以下命令来添加可执行权限：
```
chmod +x my_save.py
```
安装scipy库：在终端中运行以下命令来安装scipy库：
```
bashCopy code
pip install scipy
```
这将安装scipy库到你的Python环境中。
运行ROS节点：运行你的ROS节点，你可以使用rosrun命令，例如：

rosrun my_ros_package my_save.py

这将在ROS中运行你的Python节点，使其能够订阅和发布ROS话题，包括激光扫描数据。