思岚雷达型号C1\S2雷达类型添加角度屏蔽

使用角度屏蔽的目的，由于时间安装时，安装位置周边会有附近其他设备影响。
本次使用的雷达型号C1和S2 ， ros2 版本 https://github.com/Slamtec/sllidar_ros2 。其中主要修改两个文件： sllidar_node.cpp 以及对应的launch文件

sllidar_node.cpp

• 引入 RAD2DEG
  

#define DEG2RAD(x) ((x)*M_PI/180.)
#define RAD2DEG(x) ((x)*180./M_PI)

• init_param
  

		this->declare_parameter<bool>("cut_angle", false);
		this->declare_parameter<int>("right_degrees",180);
        this->declare_parameter<int>("left_degrees",180);
        ......
		this->get_parameter_or<bool>("cut_angle", cut_angle, false);
        this->get_parameter_or<int>("right_degrees", right_degrees, 180);
        this->get_parameter_or<int>("left_degrees", left_degrees, 180);

• publish_scan 函数 主要内容

		if(cut_angle){ // 需要进行角度屏蔽
			for (size_t i = 0; i < node_count; i++){
				scan_msg->ranges[i] = std::numeric_limits<float>::infinity();
			}
			
			if(reverse_data){
				for (size_t i = 0; i < node_count; i++)
				{
					float read_value = (float) nodes[i].dist_mm_q2/4.0f/1000;
					float get_degree = RAD2DEG(scan_msg->angle_min + scan_msg->angle_increment * i) + 180;
					if (get_degree  < right_degrees || get_degree  > left_degrees) {
						// 如果不在屏蔽范围内，继续处理数据
						if (read_value == 0.0){
							// 屏蔽操作：将距离设置为无穷大
							scan_msg->ranges[i] = std::numeric_limits<float>::infinity();
						} else {
							// 屏蔽操作：使用原始距离值
							scan_msg->ranges[i] = read_value;
						}
						scan_msg->intensities[i] = (float) (nodes[i].quality >> 2);
					} else {
						// 屏蔽操作：不使用该数据点
						scan_msg->ranges[i] = std::numeric_limits<float>::infinity();
						scan_msg->intensities[i] = 0; // 或者使用其他值来表示屏蔽的数据点
					}
				}
			} else {
				for (size_t i = 0; i < node_count; i++)
				{
					float read_value = (float) nodes[i].dist_mm_q2/4.0f/1000;
					float get_degree = RAD2DEG(scan_msg->angle_min + scan_msg->angle_increment * i) + 180;
					if (get_degree  < right_degrees || get_degree  > left_degrees){
						// 如果不在屏蔽范围内，继续处理数据
						if (read_value == 0.0)
							// 屏蔽操作：将距离设置为无穷大
							scan_msg->ranges[node_count - 1 - i] = std::numeric_limits<float>::infinity();
						else 
							// 屏蔽操作：使用原始距离值
							scan_msg->ranges[node_count - 1 - i] = read_value;
						scan_msg->intensities[node_count - 1 - i] = (float) (nodes[i].quality >> 2);
					}
				}
			}
		} else { // 不需要进行角度屏蔽
			bool reverse_data = (!inverted && reversed) || (inverted && !reversed);
            if (!reverse_data) {
                for (size_t i = 0; i < node_count; i++) {
                    float read_value = (float) nodes[i].dist_mm_q2/4.0f/1000;
                    if (read_value == 0.0)
                        scan_msg->ranges[i] = std::numeric_limits<float>::infinity();
                    else
                        scan_msg->ranges[i] = read_value;
                    scan_msg->intensities[i] = (float) (nodes[i].quality >> 2);
                }
            } else {
                for (size_t i = 0; i < node_count; i++) {
                    float read_value = (float)nodes[i].dist_mm_q2/4.0f/1000;
                    if (read_value == 0.0)
                        scan_msg->ranges[node_count-1-i] = std::numeric_limits<float>::infinity();
                    else
                        scan_msg->ranges[node_count-1-i] = read_value;
                    scan_msg->intensities[node_count-1-i] = (float) (nodes[i].quality >> 2);
                }
            }
		}

• private 生命 变量
  

	bool cut_angle = false;
    int right_degrees = 180;
    int left_degrees = 180;

对应型号的launch文件

• C1 与 S2 添加参数位置相同

def generate_launch_description():
    channel_type =  LaunchConfiguration('channel_type', default='serial')
    serial_port = LaunchConfiguration('serial_port', default='/dev/ttyUSB0')
    serial_baudrate = LaunchConfiguration('serial_baudrate', default='460800')
    frame_id = LaunchConfiguration('frame_id', default='laser')
    inverted = LaunchConfiguration('inverted', default='false')
    angle_compensate = LaunchConfiguration('angle_compensate', default='true')
    # 添加参数
    cut_angle = LaunchConfiguration('cut_angle', default='true')
    right_degrees = LaunchConfiguration('right_degrees', default='135')
    left_degrees = LaunchConfiguration('left_degrees', default='225')
    scan_mode = LaunchConfiguration('scan_mode', default='DenseBoost')
    scan_topic = LaunchConfiguration('scan_topic', default='/scan1')
    ......
	     Node(
	     package='sllidar_ros2',
	     executable='sllidar_node',
	     name='sllidar_node',
	     parameters=[{'channel_type':channel_type,
	                  'serial_port': serial_port,
	                  'serial_baudrate': serial_baudrate,
	                  'frame_id': frame_id,
	                  'inverted': inverted,
	                  'angle_compensate': angle_compensate,
	                  'cut_angle': cut_angle,
	                  'right_degrees': right_degrees,
	                  'left_degrees': left_degrees,
	                  'scan_mode': scan_mode,
	                  'scan_topic': scan_topic}],
	     output='screen'),
	])

文章参考 ：https://blog.youkuaiyun.com/qq_41854650/article/details/102740533