From 5a82ad239b3eee26b1c2a380b3e0d5eec0a7fbab Mon Sep 17 00:00:00 2001 From: NimblKun Date: Tue, 29 Mar 2022 09:42:51 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E6=AD=A3=E5=B8=A7=E5=A4=B4=E4=B8=BAPI?= =?UTF-8?q?/4<--->7PI/4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/main.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main.cpp b/src/main.cpp index d04f220..42f9333 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -1,7 +1,7 @@ #include #include #include "hins/xingsong_driver.h" - +#include using namespace hins; sensor_msgs::LaserScanPtr ToLaserScan(const ScanData &data, const float& start_angle, const float& end_angle, const std::string& frame_name); @@ -56,8 +56,8 @@ sensor_msgs::LaserScanPtr ToLaserScan(const ScanData &data, const float& start_a ret->range_min = 0.05; ret->range_max = 10.0; - ret->angle_min = M_PI/2.0f; - ret->angle_max = 3.0 * M_PI/2.0f; + ret->angle_min = M_PI/4.0f; + ret->angle_max = 7.0 * M_PI/4.0f; ret->time_increment = 1/20/float(data.distance_data.size()); // 一个周期时间除以扫描点数 @@ -71,7 +71,7 @@ sensor_msgs::LaserScanPtr ToLaserScan(const ScanData &data, const float& start_a float angle = ret->angle_min + i * ret->angle_increment; float dis = float(data.distance_data[i])/1000.0f; - + if(angle < start_angle || angle > end_angle) ret->ranges[i] = 1024.0; else