Skip to content
This repository was archived by the owner on Mar 8, 2023. It is now read-only.

Commit c6b9f8a

Browse files
michael1309michael1309
committed
angle compensation of NAV3xx optimized
1 parent df37ed2 commit c6b9f8a

File tree

10 files changed

+109
-91
lines changed

10 files changed

+109
-91
lines changed

‎CMakeLists.txt‎

Lines changed: 21 additions & 45 deletions
Original file line numberDiff line numberDiff line change
@@ -23,9 +23,12 @@ endmacro(use_cxx11)
2323

2424
# Switch on, if you use c11-specific commands
2525
use_cxx11()
26+
set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-format-overflow")
2627

2728
# set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall")
2829
# set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wpedantic")
30+
#
31+
2932

3033
project(sick_scan)
3134

@@ -48,10 +51,12 @@ project(sick_scan)
4851
# set(CATKIN_DEVEL_PREFIX "${CMAKE_BINARY_DIR}/devel")
4952
# set(CMAKE_PREFIX_PATH "${CMAKE_BINARY_DIR}/devel;/opt/ros/melodic")
5053

54+
find_package(Boost REQUIRED COMPONENTS system)
55+
5156

52-
find_package(catkin REQUIRED
53-
COMPONENTS
57+
find_package(catkin REQUIRED COMPONENTS
5458
roscpp
59+
roslib # needed ros::package::getPath()
5560
sensor_msgs
5661
diagnostic_updater
5762
dynamic_reconfigure
@@ -60,14 +65,12 @@ find_package(catkin REQUIRED
6065
sensor_msgs
6166
visualization_msgs
6267
message_generation
63-
pcl_conversions
64-
pcl_ros
6568
)
6669

6770

6871
find_package(PkgConfig REQUIRED)
6972

70-
find_package(Boost REQUIRED COMPONENTS system)
73+
7174

7275
generate_dynamic_reconfigure_options(
7376
cfg/SickScan.cfg
@@ -96,12 +99,13 @@ generate_messages(
9699
)
97100

98101
catkin_package(
99-
DEPENDS Boost
100-
CATKIN_DEPENDS message_runtime roscpp sensor_msgs diagnostic_updater dynamic_reconfigure
102+
CATKIN_DEPENDS message_runtime roscpp sensor_msgs diagnostic_updater dynamic_reconfigure perception_pcl
101103
LIBRARIES sick_scan_lib
102-
INCLUDE_DIRS include)
104+
INCLUDE_DIRS include
105+
DEPENDS Boost
106+
)
103107

104-
include_directories(include include/tinyxml ${catkin_INCLUDE_DIRS} include/sick_scan)
108+
include_directories(include include/tinyxml ${catkin_INCLUDE_DIRS} include/sick_scan)
105109

106110
add_library(sick_scan_lib
107111
driver/src/dataDumper.cpp
@@ -145,22 +149,19 @@ add_executable(radar_object_marker
145149
tools/radar_object_marker/src/radar_object_marker.cpp
146150
tools/pcl_converter/src/gnuplotPaletteReader.cpp
147151
include/radar_object_marker/radar_object_marker.h)
148-
target_link_libraries(radar_object_marker sick_scan_lib)
152+
target_link_libraries(radar_object_marker sick_scan_lib )
149153
#
150154
#
151155
#
152156

153157

154-
add_executable(pcl_converter tools/pcl_converter/src/pcl_converter.cpp tools/pcl_converter/src/gnuplotPaletteReader.cpp)
155-
156-
target_link_libraries(pcl_converter
157-
${catkin_LIBRARIES}
158-
${roslib_LIBRARIES}
159-
${PCL_LIBRARIES})
160-
161-
162-
# add_executable( pcl_find_plane tools/pcl_converter/src/pcl_find_plane.cpp)
163-
# target_link_libraries(pcl_find_plane ${catkin_LIBRARIES})
158+
#
159+
# pcl_converter disabled to avoid dependency to pcl
160+
#
161+
# add_executable(pcl_converter tools/pcl_converter/src/pcl_converter.cpp tools/pcl_converter/src/gnuplotPaletteReader.cpp)
162+
#
163+
# target_link_libraries(pcl_converter
164+
# ${catkin_LIBRARIES})
164165

165166

166167
target_link_libraries(sick_generic_caller sick_scan_lib)
@@ -177,31 +178,6 @@ target_link_libraries(sick_scan_test
177178
${roslib_LIBRARIES}
178179
sick_scan_lib)
179180

180-
#
181-
# Sensor alignment tool
182-
#
183-
# currently disabled for trusty support
184-
# add_executable(sensor_alignment
185-
# tools/sensor_alignment/sensor_alignment_tf_dyn.cpp
186-
# )
187-
#
188-
# target_link_libraries(sensor_alignment
189-
# ${catkin_LIBRARIES}
190-
# )
191-
# add_dependencies(sensor_alignment ${PROJECT_NAME}_gencfg)
192-
193-
# target_link_libraries(${PROJECT_NAME}_pointcloud2_refresh
194-
# ${catkin_LIBRARIES}
195-
# )
196-
197-
198-
# Destinations:
199-
# LIB for library
200-
# BIN for Binaries
201-
# INCLUDE for Includes
202-
# SHARE for config files, launch files etc.
203-
204-
205181
install(TARGETS sick_scan_lib
206182
DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})
207183

‎driver/src/dataDumper.cpp‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -64,6 +64,7 @@ int DataDumper::writeToFileNameWhenBufferIsFull(std::string filename)
6464

6565
int DataDumper::dumpUcharBufferToConsole(unsigned char *buffer, int bufLen)
6666
{
67+
int ret = 0;
6768
char asciiBuffer[255] = {0};
6869
for (int i = 0; i < bufLen; i++)
6970
{
@@ -94,6 +95,7 @@ int DataDumper::dumpUcharBufferToConsole(unsigned char *buffer, int bufLen)
9495

9596
printf("%s\n", asciiBuffer);
9697
}
98+
return(ret);
9799
}
98100

99101
int DataDumper::testbed()

‎driver/src/helper/angle_compensator.cpp‎

Lines changed: 36 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -97,6 +97,40 @@ using namespace std;
9797
*/
9898

9999

100+
/*!
101+
\brief Compensate raw angle given in [RAD] in the ROS axis orientation system
102+
\param angleInRad: raw angle in [RAD] (
103+
*/
104+
double AngleCompensator::compensateAngleInRadFromRos(double angleInRadFromRos)
105+
{
106+
// this is a NAV3xx - X-Axis is the same like ROS
107+
// but we rotate clockwise instead of counter clockwise
108+
double angleInRadFromSickOrg = 0.0;
109+
double angleInRadToRosCompensated = 0.0;
110+
if (useNegSign)
111+
{
112+
angleInRadFromSickOrg = -angleInRadFromRos;
113+
}
114+
else // NAV2xx
115+
{
116+
// NAV2xx uses "standard" counter clockwise rotation like ROS, but X-Axis shows to the right
117+
angleInRadFromSickOrg = angleInRadFromRos + M_PI/2.0;
118+
}
119+
120+
double angleInRadFromSickCompensated = compensateAngleInRad(angleInRadFromSickOrg);
121+
122+
if (useNegSign) // NAV3xx
123+
{
124+
angleInRadToRosCompensated = -angleInRadFromSickCompensated;
125+
}
126+
else // NAV2xx
127+
{
128+
// NAV2xx uses "standard" counter clockwise rotation like ROS
129+
angleInRadToRosCompensated = angleInRadFromSickCompensated - M_PI/2.0;
130+
}
131+
return(angleInRadToRosCompensated);
132+
}
133+
100134
/*!
101135
\brief Compensate raw angle given in [RAD]
102136
\param angleInRad: raw angle in [RAD]
@@ -109,6 +143,7 @@ double AngleCompensator::compensateAngleInRad(double angleInRad)
109143
{
110144
sign = -1;
111145
}
146+
//angleInRad *= sign;
112147
double angleCompInRad = angleInRad + deg2radFactor * amplCorr * sin(angleInRad + sign * phaseCorrInRad) + offsetCorrInRad;
113148
return(angleCompInRad);
114149
}
@@ -124,6 +159,7 @@ double AngleCompensator::compensateAngleInDeg(double angleInDeg)
124159
{
125160
sign = -1;
126161
}
162+
//angleInDeg*=sign;
127163
// AngleComp =AngleRaw + AngleCompAmpl * SIN( AngleRaw + AngleCompPhase ) + AngleCompOffset
128164
double angleCompInDeg;
129165
double deg2radFactor = 0.01745329252; // pi/180 deg - see for example: https://www.rapidtables.com/convert/number/degrees-to-radians.html

‎driver/src/sick_generic_caller.cpp‎

Lines changed: 4 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -120,10 +120,13 @@
120120
// 1.7.2: 2020-06-09: TiM433 added and launch file info for TiM4xx added
121121
// 1.7.3: 2020-06-10: NAV 3xx angle correction added
122122
// 1.7.4: 2020-06-10: NAV 3xx angle correction improved
123+
// 1.7.5: 2020-06-25: Preparing for Release Noetic
124+
// 1.7.6: 2020-02-07: NAV310 handling optimized
125+
123126

124127
#define SICK_GENERIC_MAJOR_VER "1"
125128
#define SICK_GENERIC_MINOR_VER "7"
126-
#define SICK_GENERIC_PATCH_LEVEL "4"
129+
#define SICK_GENERIC_PATCH_LEVEL "6"
127130

128131
#include <algorithm> // for std::min
129132

‎driver/src/sick_generic_parser.cpp‎

Lines changed: 20 additions & 20 deletions
Original file line numberDiff line numberDiff line change
@@ -267,19 +267,19 @@ namespace sick_scan
267267
\param _scanMirrored: false for normal mounting true for up side down or NAV 310
268268
\sa setScanMirrored
269269
*/
270-
void ScannerBasicParam::setScanMirrored(bool _scannMirrored)
270+
void ScannerBasicParam::setScanMirroredAndShifted(bool _scannMirroredAndShifted)
271271
{
272-
scanMirrored = _scannMirrored;
272+
scanMirroredAndShifted = _scannMirroredAndShifted;
273273
}
274274

275275
/*!
276276
\brief flag to mark mirroring of rotation direction
277277
\param _scanMirrored: false for normal mounting true for up side down or NAV 310
278278
\sa getScanMirrored
279279
*/
280-
bool ScannerBasicParam::getScanMirrored(void)
280+
bool ScannerBasicParam::getScanMirroredAndShifted(void)
281281
{
282-
return (scanMirrored);
282+
return (scanMirroredAndShifted);
283283
}
284284

285285
/*!
@@ -409,7 +409,7 @@ namespace sick_scan
409409
basicParams[i].setDeviceIsRadar(false); // Default
410410
basicParams[i].setUseSafetyPasWD(false); // Default
411411
basicParams[i].setEncoderMode(-1); // Default
412-
basicParams[i].setScanMirrored(false);
412+
basicParams[i].setScanMirroredAndShifted(false);
413413
}
414414
if (basicParams[i].getScannerName().compare(SICK_SCANNER_LMS_1XXX_NAME) ==
415415
0) // LMS1000 - 4 layer, 1101 shots per scan
@@ -424,7 +424,7 @@ namespace sick_scan
424424
basicParams[i].setDeviceIsRadar(false); // Default
425425
basicParams[i].setUseSafetyPasWD(false); // Default
426426
basicParams[i].setEncoderMode(-1); // Default
427-
basicParams[i].setScanMirrored(false);
427+
basicParams[i].setScanMirroredAndShifted(false);
428428
}
429429
if (basicParams[i].getScannerName().compare(SICK_SCANNER_TIM_240_NAME) ==
430430
0) // TIM_5xx - 1 Layer, max. 811 shots per scan
@@ -438,7 +438,7 @@ namespace sick_scan
438438
basicParams[i].setDeviceIsRadar(false); // Default
439439
basicParams[i].setUseSafetyPasWD(false); // Default
440440
basicParams[i].setEncoderMode(-1); // Default
441-
basicParams[i].setScanMirrored(false);
441+
basicParams[i].setScanMirroredAndShifted(false);
442442

443443
}
444444
if (basicParams[i].getScannerName().compare(SICK_SCANNER_TIM_5XX_NAME) ==
@@ -453,7 +453,7 @@ namespace sick_scan
453453
basicParams[i].setDeviceIsRadar(false); // Default
454454
basicParams[i].setUseSafetyPasWD(false); // Default
455455
basicParams[i].setEncoderMode(-1); // Default
456-
basicParams[i].setScanMirrored(false);
456+
basicParams[i].setScanMirroredAndShifted(false);
457457

458458
}
459459
if (basicParams[i].getScannerName().compare(SICK_SCANNER_LMS_4XXX_NAME) == 0) // LMS_4xxx - 1 Layer, 600 Hz
@@ -467,7 +467,7 @@ namespace sick_scan
467467
basicParams[i].setDeviceIsRadar(false); // Default
468468
basicParams[i].setUseSafetyPasWD(false); // Default
469469
basicParams[i].setEncoderMode(-1); // Default
470-
basicParams[i].setScanMirrored(false);
470+
basicParams[i].setScanMirroredAndShifted(false);
471471
}
472472
if (basicParams[i].getScannerName().compare(SICK_SCANNER_TIM_7XX_NAME) == 0) // TIM_7xx - 1 Layer Scanner
473473
{
@@ -480,7 +480,7 @@ namespace sick_scan
480480
basicParams[i].setDeviceIsRadar(false); // Default
481481
basicParams[i].setUseSafetyPasWD(false); // Default
482482
basicParams[i].setEncoderMode(-1); // Default
483-
basicParams[i].setScanMirrored(false);
483+
basicParams[i].setScanMirroredAndShifted(false);
484484
}
485485
if (basicParams[i].getScannerName().compare(SICK_SCANNER_TIM_7XXS_NAME) == 0) // TIM_7xxS - 1 layer Safety Scanner
486486
{
@@ -493,7 +493,7 @@ namespace sick_scan
493493
basicParams[i].setDeviceIsRadar(false); // Default
494494
basicParams[i].setUseSafetyPasWD(true); // Safety scanner
495495
basicParams[i].setEncoderMode(-1); // Default
496-
basicParams[i].setScanMirrored(false);
496+
basicParams[i].setScanMirroredAndShifted(false);
497497
}
498498
if (basicParams[i].getScannerName().compare(SICK_SCANNER_LMS_5XX_NAME) == 0) // LMS_5xx - 1 Layer
499499
{
@@ -506,7 +506,7 @@ namespace sick_scan
506506
basicParams[i].setDeviceIsRadar(false); // Default
507507
basicParams[i].setUseSafetyPasWD(false); // Default
508508
basicParams[i].setEncoderMode(-1); // Default
509-
basicParams[i].setScanMirrored(false);
509+
basicParams[i].setScanMirroredAndShifted(false);
510510
}
511511
if (basicParams[i].getScannerName().compare(SICK_SCANNER_LMS_1XX_NAME) == 0) // LMS_1xx - 1 Layer
512512
{
@@ -519,7 +519,7 @@ namespace sick_scan
519519
basicParams[i].setDeviceIsRadar(false); // Default
520520
basicParams[i].setUseSafetyPasWD(false); // Default
521521
basicParams[i].setEncoderMode(-1); // Default
522-
basicParams[i].setScanMirrored(false);
522+
basicParams[i].setScanMirroredAndShifted(false);
523523
}
524524
if (basicParams[i].getScannerName().compare(SICK_SCANNER_MRS_6XXX_NAME) == 0) //
525525
{
@@ -533,7 +533,7 @@ namespace sick_scan
533533
basicParams[i].setDeviceIsRadar(false); // Default
534534
basicParams[i].setUseSafetyPasWD(false); // Default
535535
basicParams[i].setEncoderMode(-1); // Default
536-
basicParams[i].setScanMirrored(false);
536+
basicParams[i].setScanMirroredAndShifted(false);
537537
}
538538

539539
if (basicParams[i].getScannerName().compare(SICK_SCANNER_RMS_3XX_NAME) == 0) // Radar
@@ -548,7 +548,7 @@ namespace sick_scan
548548
basicParams[i].setDeviceIsRadar(true); // Device is a radar
549549
basicParams[i].setUseSafetyPasWD(false); // Default
550550
basicParams[i].setEncoderMode(-1); // Default
551-
basicParams[i].setScanMirrored(false);
551+
basicParams[i].setScanMirroredAndShifted(false);
552552
}
553553
if (basicParams[i].getScannerName().compare(SICK_SCANNER_NAV_3XX_NAME) == 0) // Nav 3xx
554554
{
@@ -559,7 +559,7 @@ namespace sick_scan
559559
basicParams[i].setExpectedFrequency(55.0);
560560
basicParams[i].setUseBinaryProtocol(true);
561561
basicParams[i].setDeviceIsRadar(false); // Default
562-
basicParams[i].setScanMirrored(true); // other ortation direction than other scanners
562+
basicParams[i].setScanMirroredAndShifted(true);
563563
}
564564
if (basicParams[i].getScannerName().compare(SICK_SCANNER_NAV_2XX_NAME) == 0) // NAV_2xx - 1 Layer
565565
{
@@ -572,7 +572,7 @@ namespace sick_scan
572572
basicParams[i].setDeviceIsRadar(false); // Default
573573
basicParams[i].setUseSafetyPasWD(false); // Default
574574
basicParams[i].setEncoderMode(-1); // Default
575-
basicParams[i].setScanMirrored(false);
575+
basicParams[i].setScanMirroredAndShifted(false);
576576
}
577577
if (basicParams[i].getScannerName().compare(SICK_SCANNER_TIM_4XX_NAME) == 0) // TiM433 and TiM443
578578
{
@@ -585,7 +585,7 @@ namespace sick_scan
585585
basicParams[i].setDeviceIsRadar(false); // Default
586586
basicParams[i].setUseSafetyPasWD(false); // Default
587587
basicParams[i].setEncoderMode(-1); // Default
588-
basicParams[i].setScanMirrored(false);
588+
basicParams[i].setScanMirroredAndShifted(false);
589589
}
590590
}
591591

@@ -801,7 +801,7 @@ namespace sick_scan
801801
if (verboseLevel > 0)
802802
{
803803
static int cnt = 0;
804-
char szDumpFileName[255] = {0};
804+
char szDumpFileName[511] = {0};
805805
char szDir[255] = {0};
806806
#ifdef _MSC_VER
807807
strcpy(szDir,"C:\\temp\\");
@@ -845,7 +845,7 @@ namespace sick_scan
845845
if (verboseLevel > 0)
846846
{
847847
static int cnt = 0;
848-
char szDumpFileName[255] = {0};
848+
char szDumpFileName[511] = {0};
849849
char szDir[255] = {0};
850850
#ifdef _MSC_VER
851851
strcpy(szDir,"C:\\temp\\");

0 commit comments

Comments
 (0)