nav310 angle setting compability

[hilmi-ica]

Hey!
I was trying to use NAV310 and ROS package laser_scan_matcher to do a localization as an odometry. Here is my launch file:

<launch>

  <param name="/use_sim_time" value="false"/>

  <node pkg="tf" type="static_transform_publisher" name="base_link_to_laser" args="0.0 0.0 0.0 0.0 0.0 0.0 base_link cloud 100" />

  <node pkg="laser_scan_matcher" type="laser_scan_matcher_node" 
    name="laser_scan_matcher_node" output="screen">
    <remap from="scan" to="/sick_nav_3xx/scan"/>
    <param name="fixed_frame" value = "odom"/>
    <param name="base_frame" value = "base_link"/>
    <param name="publish_tf" value="true"/>
    <param name="publish_odom" value="true"/>
    <param name="use_alpha_beta" value="true"/>
    <param name="max_iterations" value="10"/>
  </node>

</launch>

But, i encountered this error:

[ WARN] [1680068632.770143301]: Error in scan matching
:err: Strange FOV: -6.278822 rad = -359.750011 deg

with min_ang and max_ang:

~$ rosparam get /sick_nav_3xx/min_ang 
0.0

~$ rosparam get /sick_nav_3xx/min_ang 
6.283185307179586

So, i was trying to find a workaround by editing the code on file sick_scan_common.cpp in line 491:

if (parser_->getCurrentParamPtr()->getScannerName().compare(SICK_SCANNER_NAV_3XX_NAME) == 0)
{
  // NAV-310 only supports min/max angles of -PI to +PI (resp. 0 to 360 degree in sensor coordinates). To avoid unexpected results, min/max angles can not be set by configuration.
  config_.min_ang = -M_PI;
  config_.max_ang = +M_PI;
}

and line 2372:

if (parser_->getCurrentParamPtr()->getScannerName().compare(SICK_SCANNER_NAV_3XX_NAME) == 0) // map ros start/stop angle to NAV-3xx logic
{
  // NAV-310 only supports min/max angles 0 to 360 degree in sensor coordinates. To avoid unexpected results, min/max angles can not be set by configuration.
  start_ang_rad = 0; // this->config_.min_ang;
  stop_ang_rad = 2 * M_PI; // this->config_.max_ang;
}

But, so far, i wasn't able to find the solution for that error.
Hope anyone can help me. Thanks!

[michael1309]

Hi,

did you use the most recent version of the sick_scan_xd-driver?
Could you see the scan in rviz in the correct manner?

I think you have a PI wrapping problem somewhere in the processing chain where, for example, 2PI is converted to -2PI. As a workaround you can e.g. build a small ROS node that receives the scan messages and limits them e.g. to the angle range from -150 degrees to +150 degrees and sends them as a new scan message. This then avoids the pi-wrapping problem.

[hilmi-ica]

Thanks for the respond!
Yes, i build from the source about a week ago.
The scan is good as you can see in this pic:

I did try to work around it by changing the parameters to:

  start_ang_rad = M_PI/2; // this->config_.min_ang;
  stop_ang_rad = 3/2*M_PI; // this->config_.max_ang;

and it shows in the rosparam that:

~$ rosparam get /sick_nav_3xx/min_ang 
1.5707963268

~$ rosparam get /sick_nav_3xx/max_ang
4.71238898039

But, it still results in the same error and the same number:

[ WARN] [1679891634.121426056]: Error in scan matching
:err: Strange FOV: -6.278822 rad = -359.750011 deg

For making a new ROS node, i'm still a beginner on programming especially in ROS. So, if you can guide me that would be helpful.
Another interesting thing and maybe this can become a clue. When i use the ascii protocol by launching:
roslaunch sick_scan sick_nav_3xx_ascii.launch
The laser_match_scanner works but the scan is kind of small and the direction of the odom is rotated by 90 degree to the left:

But, with the warning:
[ WARN] [1679892285.005301187]: Intensity parameter is enabled, but the scanner is not configured to send RSSI values!

[rostest]

Thanks for your feedback! We checked the pointcloud and laserscan messages against our NAV-310 with firmware 1.50 using sick_scan_xd release 2.8.15 (https://github.com/SICKAG/sick_scan_xd/tree/master). rivz shows the correct orientation (example screenshot):

ros command rostopic echo -n 1 /sick_nav_3xx/scan shows that the angle of the laserscan messages are within -180 to +180 degree in ros coordinates as expected, e.g.:

header: 
  seq: 492
  stamp: 
    secs: 1680170796
    nsecs:  85797309
  frame_id: "cloud"
angle_min: -3.1415927410125732
angle_max: 3.1415927410125732
angle_increment: -0.004363323096185923
time_increment: 8.695651922607794e-05
scan_time: 0.125
range_min: 0.0
range_max: 100.0

ros commands rosparam get /sick_nav_3xx/min_ang resp. rosparam get /sick_nav_3xx/max_ang display the expected azimuth angles in sensor coordinates (0 to 360 degree):

rosparam get /sick_nav_3xx/min_ang
0.0
rosparam get /sick_nav_3xx/max_ang
6.283185307179586

These values are the expected ones. It looks like your laserscan messages might be rotated for some reason. Please run the following steps:

• Reset the lidar to factory defaults.
• Update the lidar firmware, if a new version is available by SOPAS ET.
• Rebuild sick_scan_xd using the most recent release 2.8.15 and rerun roslaunch sick_scan sick_nav_3xx.launch.
• Run rostopic echo -n 1 /sick_nav_3xx/scan and check values angle_min, angle_max and angle_increment. They should be (almost) identical to the above values.
• Please post a complete logfile or console screenshot from the driver output of the initial startup sequence.

We attach the logfile from our run for further information. nav-310.log

[hilmi-ica]

I still got the same error from the launch file above:

[ WARN] [1680328402.812593217]: Error in scan matching
^C:err: Strange FOV: -6.278822 rad = -359.750011 deg

And here is the message from the command rostopic echo -n 1 /sick_nav_3xx/scan:

header: 
  seq: 2593
  stamp: 
    secs: 1680328713
    nsecs: 700240135
  frame_id: "cloud"
angle_min: -3.1415927410125732
angle_max: 3.1415927410125732
angle_increment: -0.004363323096185923
time_increment: 8.695651922607794e-05
scan_time: 0.125
range_min: 0.0
range_max: 100.0

And:

rosparam get /sick_nav_3xx/min_ang 
0.0
rosparam get /sick_nav_3xx/max_ang 
6.283185307179586

And with the logfile nav2.log
In Rviz, the laserscan is also showing the correct orientation:

[michael1309]

As written above, we think this is a numerical problem in the navigation software. The difference between start angle and stop angle in the scan message is a little larger than 2 *Pi due to numerical inaccuracies in the float representation:

(((3.1415927410125732 - -3.1415927410125732) / (2*pi) = 1.00000002783

In principle, this is not a problem. However, this case is not provided for in the navigation software.

The calculation (((3.1415927410125732 - -3.1415927410125732) - 4 *pi
results exactly in the FOV value you see in the navigation software, namely:

-6.28318513233

To change this behavior you can
a) patch the navigation software
b) create a ROS node that pre-filters the scan and e.g. crops it at the start of scan and end of scan.
c) patch sick_scan_xd to optimize this behavior here.

Even though we recommend b), you could start with c) for a pre-test by modifying angle_min and angle_max in the scan message header so that the difference is less than 2 * pi, e.g.

angle_min := 0.9999 * angle_min
angle_max := 0.9999 * angle_max
angle_increment := 0.9999 * angle_increment

The difference between angle_min and angle_max that results is then slightly smaller than 2 * pi. So the pi wrapping problem should not occur anymore. The minimum deviation lies in the size of the measurement noise and therefore, in my opinion, has no influence on further processing.

With this method, the scan message would then be as follows (see modification for the three mentioned values).

header: 
  seq: 2593
  stamp: 
    secs: 1680328713
    nsecs: 700240135
  frame_id: "cloud
angle_min: -3.14127858174
angle_max:3.14127858174
angle_increment: -0.00436288676
time_increment: 8.695651922607794e-05
scan_time: 0.125
range_min: 0.0
range_max: 100.0

Good luck!

[hilmi-ica]

Can you guide me how to do c). For b) i'm trying to use package laser_filters, but its not working. Where do i need to change the angle_min, angle_max values?

[michael1309]

I would start around these lines:

                              msg.angle_min = startAngle / 180.0 * M_PI + parser_->getCurrentParamPtr()->getScanAngleShift(); // msg.angle_min = startAngle / 180.0 * M_PI - M_PI / 2;
                              msg.angle_increment = sizeOfSingleAngularStep;
                              msg.angle_max = msg.angle_min + (numberOfItems - 1) * msg.angle_increment;

by adding the following three lines for the patch:

double patch_factor = 0.9999;
msg.angle_min *= patch_factor;
msg.angle_increment *= patch_factor;
msg.angle_max *=  patch_factor;

[hilmi-ica]

Sorry, i couldn't find those line in sick_scan_common.cpp. All i could find is:
4533 angle = msg.angle_min - angleShift;
and
4634 angle += msg.angle_increment;
Sorry again, if i'm slow to get what you're trying to say.

Edit:
I found those line in sick_lmd_scandata_parser.cpp:

476                              msg.angle_min = startAngle / 180.0 * M_PI + parser_->getCurrentParamPtr()->getScanAngleShift(); // msg.angle_min = startAngle / 180.0 * M_PI - M_PI / 2;
477                              msg.angle_increment = sizeOfSingleAngularStep;
478                              msg.angle_max = msg.angle_min + (numberOfItems - 1) * msg.angle_increment;

And similar line in sick_generic_parser.cpp:

1449    int starting_angle = -1;
1450    sscanf(fields[23], "%x", &starting_angle);
1451    msg.angle_min = (float)((starting_angle / 10000.0) / 180.0 * M_PI - M_PI / 2);
1452    // ROS_DEBUG("starting_angle: %d, angle_min: %f", starting_angle, msg.angle_min);
1453
1454    // 24: Angular step width (2710)
1455    unsigned short angular_step_width = -1;
1456    sscanf(fields[24], "%hx", &angular_step_width);
1457    msg.angle_increment = (angular_step_width / 10000.0) / 180.0 * M_PI;
1458    msg.angle_max = (float)(msg.angle_min + (number_of_data - 1) * msg.angle_increment);

Do i need to edit both of those

[hilmi-ica]

I would start around these lines:

                              msg.angle_min = startAngle / 180.0 * M_PI + parser_->getCurrentParamPtr()->getScanAngleShift(); // msg.angle_min = startAngle / 180.0 * M_PI - M_PI / 2;
                              msg.angle_increment = sizeOfSingleAngularStep;
                              msg.angle_max = msg.angle_min + (numberOfItems - 1) * msg.angle_increment;

by adding the following three lines for the patch:

double patch_factor = 0.9999;
msg.angle_min *= patch_factor;
msg.angle_increment *= patch_factor;
msg.angle_max *=  patch_factor;

This didn't do anything. It results with the same output as above and error as above:

rostopic echo -n 1 /sick_nav_3xx/scan 
header: 
  seq: 22
  stamp: 
    secs: 1680496733
    nsecs: 814131498
  frame_id: "cloud"
angle_min: -3.1415927410125732
angle_max: 3.1415927410125732
angle_increment: -0.004319689702242613
time_increment: 8.695651922607794e-05
scan_time: 0.125
range_min: 0.0
range_max: 100.0

The only change is the angle_increment. When i change the patch factor to 0.7 it results in:

rostopic echo -n 1 /sick_nav_3xx/scan 
header: 
  seq: 135
  stamp: 
    secs: 1680497877
    nsecs: 852567672
  frame_id: "cloud"
angle_min: -3.1415927410125732
angle_max: 3.1415927410125732
angle_increment: -0.003054326167330146
time_increment: 8.695651922607794e-05
scan_time: 0.125
range_min: 0.0
range_max: 100.0

Again, the change is only at angle_increment. And the odometry error shows:

[ WARN] [1680497898.171055410]: Error in scan matching
^C:err: Strange FOV: -4.395175 rad = -251.824990 deg

For note, i already did:

catkin_make_isolated
source devel_isolated/setup.bash

Thanks for the respond! Yes, i build from the source about a week ago. The scan is good as you can see in this pic: I did try to work around it by changing the parameters to:

  start_ang_rad = M_PI/2; // this->config_.min_ang;
  stop_ang_rad = 3/2*M_PI; // this->config_.max_ang;

and it shows in the rosparam that:

~$ rosparam get /sick_nav_3xx/min_ang 
1.5707963268

~$ rosparam get /sick_nav_3xx/max_ang
4.71238898039

But, it still results in the same error and the same number:

[ WARN] [1679891634.121426056]: Error in scan matching
:err: Strange FOV: -6.278822 rad = -359.750011 deg

For making a new ROS node, i'm still a beginner on programming especially in ROS. So, if you can guide me that would be helpful. Another interesting thing and maybe this can become a clue. When i use the ascii protocol by launching: roslaunch sick_scan sick_nav_3xx_ascii.launch The laser_match_scanner works but the scan is kind of small and the direction of the odom is rotated by 90 degree to the left: But, with the warning: [ WARN] [1679892285.005301187]: Intensity parameter is enabled, but the scanner is not configured to send RSSI values!

Doesn't this reply give some hints? my ascii launch work fine and can be transformed to odometry by the launch file above. The issue with this launch file is that the size of the laserscan is relatively small and if the red axes show the front of the lidar, its mean that the laserscan have been rotated about 90 degrees, like fig below:

And rostopic echo -n 1 /sick_nav_3xx/scan shows:

rostopic echo -n 1 /sick_nav_3xx/scan 
header: 
  seq: 50
  stamp: 
    secs: 1680498453
    nsecs: 360134257
  frame_id: "cloud"
angle_min: -1.5707963705062866
angle_max: 4.7080254554748535
angle_increment: 0.004363323096185923
time_increment: 8.695651922607794e-05
scan_time: 0.125
range_min: 0.0
range_max: 100.0

[michael1309]

Thanks for your further investigation. We will give a more detailed feedback at latest until tomorrow.

[hilmi-ica]

Thanks for being so patience an attentive to my problems! Can't wait for the feedback!

[michael1309]

Hello,

we have now set up a own branch for follow up.
Please clone the branch 159-nav310-angle-setting-compability
with the command

git clone -b 159-nav310-angle-setting-compability https://github.com/SICKAG/sick_scan_xd

In the branch we have now made sure that
a) the range of 2 * PI is not exceeded
b) the logic for angle_min, angle_max, angle_increment is consistent.

Cf. the following output:

Input:

rostopic echo /sick_nav_3xx/scan|grep header -A 12 -B 1

Output:

---
header: 
  seq: 3580
  stamp: 
    secs: 1680551401
    nsecs: 842907905
  frame_id: "cloud"
angle_min: 3.141592264175415
angle_max: -3.141592264175415
angle_increment: -0.004366354551166296
time_increment: 8.695651922607794e-05
scan_time: 0.125
range_min: 0.0
range_max: 100.0
--

According to ROS logic, the value "angle_min" describes the start value of the scan. Since the lidar rotates clockwise, this value must be in this case greater than "angle_max". A total of 1440 values are output during a scan.

Proof:

angle_max = angle_min + (1440 - 1) * angle_increment = 3.141592264175415 + (1440 - 1) * -0.004366354551166296 = -3.14159193495. That fits.

[hilmi-ica]

Hey i tried the branch, and the laser scan output is as expected:

header: 
  seq: 114
  stamp: 
    secs: 1680591868
    nsecs: 283500194
  frame_id: "cloud"
angle_min: 3.141592264175415
angle_max: -3.141592264175415
angle_increment: -0.004366354551166296
time_increment: 8.695651922607794e-05
scan_time: 0.125
range_min: 0.0
range_max: 100.0
--
---

But it still give the same error, that is:

[ WARN] [1680591897.618700189]: Error in scan matching
:err: Strange FOV: -6.283184 rad = -359.999928 deg

But, there is one thing that is working. the laser_filters, which the problem can be read in this link, has work!:

header: 
  seq: 26
  stamp: 
    secs: 1680592184
    nsecs: 103201073
  frame_id: "cloud"
angle_min: 1.5697046518325806
angle_max: -1.569704294204712
angle_increment: -0.004366354551166296
time_increment: 8.695651922607794e-05
scan_time: 0.125
range_min: 0.0
range_max: 100.0

But, it still gave the same error in odometry but with the lower FOV:

[ WARN] [1680592326.291168626]: Error in scan matching
:err: Strange FOV: -3.139409 rad = -179.874876 deg

Is the laser_scan_matcher inverts the scan angle value? if we invert it the driver will the orientation of the lidar also be inverted if it works?

[michael1309]

Great that now the consistency of the data is correct.
The problem now lies in your processing chain, because it cannot cope with the fact that the calculation (angle_max - angle_min) returns a negative value.

You can help yourself as follows:

Solution I:

Invert angle_min, angle_max and angle_increment so that the values look like this:

angle_min: -1.5697046518325806
angle_max: 1.569704294204712
angle_increment: 0.004366354551166296

Interpret this modification as if a lidar rotating counterclockwise is mounted upside down. This means that you may then have to rotate the values from the laser scan 180 degrees around the X axis again later to bring them in line with the carrier system.

If you want to avoid this, you can alternatively try the following:

Solution II:

• Change angle_min, angle_max, angle_increment as follows:

angle_min: -1.5697046518325806
angle_max: 1.569704294204712
angle_increment: 0.004366354551166296

• Sort the range values in reverse order before the scan output.
  You could also implement this e.g. in your laser scan filter.
  Attention: With this trick the time stamp is no longer correct. You would have to
  calculate the time stamp of the last shot in the scan and then enter a negative time increment. However, this may not be critical for your application.

[michael1309]

With these notes, I would close the discussion for now, since in our opinion the driver is now working correctly.

It looks like the laser_scan_matcher has an issue with clockwise rotated laser scans with angle_max < angle_min and angle_increment < 0. According to the documentation, the laser_scan_matcher can use the cartesian pointcloud as well. We recommend to use the PointCloud2 messages to avoid angle issues resp. ambiguities due to 360 degree wrap around. The default PointCloud2 topic is "cloud" and can be configured in the launchfile or it can be overwritten by commandline parameter.

[hilmi-ica]

Thanks for the help!!