jonbinney
commented
9 years ago Looks Upside-down logic is here: https://github.com/ros-perception/slam_karto/blob/54b3e24c4a4565d527d81e1f1bd262a08d99ac7c/src/slam_karto.cpp#L363
I'm guessing the problem is that when converting to quaternion and back, the min and max angles get changed, because of the range of tf::getYaw().
An alternative method to detect upside-down-ness would be to create a unit vector along the laser's z-axis, then transform that into base_link using tf, and check whether the dot product of the transformed vector with a unit z vector is positive or negative.
I don't have time to code that though.
mgerdzhev
using a properly mounted laser scanner slam_karto detects that it is upside down when it is clearly not. laser scan is shown correctly in rviz, but map gets flipped..
here is the laserscan header for a hokuyo laser: header: seq: 879 stamp: secs: 1446841560 nsecs: 750396458 frame_id: rear_laser angle_min: -2.27029156685 angle_max: 2.2641556263 angle_increment: 0.00613592332229 time_increment: 6.51041700621e-05 scan_time: 0.133333340287 range_min: 0.0299999993294 range_max: 11.0
and the output when starting slam_karto: [ INFO] [1446841728.498826717]: laser rear_laser's pose wrt base: -0.230 -0.013 3.115 [ INFO] [1446841728.498961823]: laser is mounted upside-down Info: clipped range threshold to be within minimum and maximum range!
Likewise if I publish a transformed laser scan message in base link with the following header I get the same upside down problem:
header: seq: 2999 stamp: secs: 1446841845 nsecs: 431243896 frame_id: base_link angle_min: -3.13759255409 angle_max: 3.14159274101 angle_increment: 0.00400000018999 time_increment: 0.0 scan_time: 0.00999999977648 range_min: 0.20000000298 range_max: 9.5
[ INFO] [1446841912.267470740]: laser base_link's pose wrt base: 0.000 0.000 0.000 [ INFO] [1446841912.267858238]: laser is mounted upside-down Info: clipped range threshold to be within minimum and maximum range!