使用角度屏蔽的目的，由于时间安装时，安装位置周边会有附近其他设备影响。
本次使用的雷达型号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'),
])