Using laser_scan_matcher with long range 2D scanner mounted not horizontally

asked 2019-04-05 10:53:16 -0500

Ganesh_Kumar gravatar image

updated 2019-04-06 04:18:13 -0500

Hello, I would like to use laser scans data recorded from a 2D scanner into laser_scan_matcher package in order to get a pose2D or odometry (purely laser odometry). (Ros kinetic on Ubuntu 16.04) The scanner is mounted on the top of a vehicle at a particular orientation. We have a transformation matrix from the laser coordinate frame to the base frame (fixed to the vehicle) call it C_socs2base. I am using a launch file created from the demo launch file of the package.

Problem: I see that the base frame is not moving significantly and the trajectory is very short just centered around the origin. I am not getting any meaningful pose/trajectory.

Questions: Q1. Can laser_scan_matcher package work fine with a scanner sensor which is not mounted horizontally to the base coordinate? Note: The maximum range of the scanner is upto 200m

Q2. Do I need to initialize the first pose of the robot (base coordinate) in the world frame? If yes then how? I have done this step as discussed below: We have an initial pose of the base frame with respect to the world frame (x,y,z and roll,pitch, yaw at time 0 as t_0 ).

I edited the source code of the package in laser_scan_matcher.cpp where I replaced f2b_.setIdentity() and f2b_kf_.setIdentity() with the following codes:

tf::Quaternion q;

q.setOrigin(Vector3(x,y,z at t_0));

q.setRPY(roll, pitch, yaw at t_0);

f2b_.setRotation(q);

f2b_kf_ =  f2b_;enter code here`

Here is the launch file:

<launch>
  <arg name="IS_TWISTSTAMPED" default="true" />
  <arg name="use_rviz" default="true" />
  <arg name="publish_covariance" default="false"/>

  <!--set up data playback from bag /-->
  <param name="/use_sim_time" value="true"/>
  <param name="/stamped_vel" value="$(arg IS_TWISTSTAMPED)"/>

  <group if="$(arg use_rviz)">
    <node pkg="rviz" type="rviz" name="rviz"
          args="-d $(find laser_scan_matcher)/demo/demo.rviz"/>
  </group>


   <!-- Odometry from pose2D /-->
  <node pkg="pointcloud2_riegl" type="pose2odom" name="pose2odom" output="screen"/>

  <!--publish an example base_link -> laser transform /-->

  <node name = "SM_laser_bl" pkg="tf" type="static_transform_publisher" args="0.0772 -0.3409 -0.1009 0.4435054 0.7681739 -0.399886 -0.2308743 base_link laser 40"/>

  <!--start the laser scan_matcher -------------------------/>

  <group if="$(arg publish_covariance)">
    <param name="laser_scan_matcher_node/do_compute_covariance" value="1"/>
    <param name="laser_scan_matcher_node/publish_pose_with_covariance" value="true"/>
    <param name="laser_scan_matcher_node/publish_pose_with_covariance_stamped" value="true"/>
  </group>
  <node pkg="laser_scan_matcher" type="laser_scan_matcher_node"
    name="laser_scan_matcher_node" output="screen">
    <remap from="scan" to="laser_scan" />
    <param name="max_iterations" value="10"/>
    <param name="publish_pose_stamped" value="true"/>

  </node>

</launch>

Thanks!

edit retag flag offensive close merge delete

Comments

When you say your laser scanner is not mounted horizontally what do you mean? Is it vertical or slightly off horizontal? However the laser scan matcher is designed to work with horizontal scans, it fundamentally won't work with inclined scans because features in the scan will not be moving in the same way as they do relative to the robot itself. Are you able to mount the sensor so it is horizontal?

PeteBlackerThe3rd gravatar image PeteBlackerThe3rd  ( 2019-04-06 04:20:36 -0500 )edit

Thank you very much for your reply and your edits. The scanner is not vertically mounted. It is inclined at significant degrees in all the axis (roll, pitch yaw) please see the tf node SM_laser_bl (static transform publisher for laser to base_link).

Q1. If the laser_scan_matcher package is not working for inclined scanners, what is the purpose of tf node (laser to base_link) as required input to the package?

Q2. Do you know (or tested) any other ROS packages or generic C++ library for my purpose? May be Google Cartograher?

I do not have access to the real hardware so I cannot make any changes. Thanks!!

Ganesh_Kumar gravatar image Ganesh_Kumar  ( 2019-04-08 03:56:50 -0500 )edit

For all practical purposes the transform laser to base link is used to determine the X Y offset between the rotation sensor of the robot and the lidar. It also determines the yaw rotation between zero degrees on the lidar and the x axis on the robot body frame, as well as if the lidar is mounted upside down or not.

The problem of an inclined 2D scanner is that as the robot rotates and translates different obstacles will go into and out of view of the lidar, so consistent 2D obstacle mapping and detection is impossible. Your only choice will be to try and build up a 3D map by estimating the robot's motion with the lidar combined with additional sensors, but I don't know of any off the shelf packages that can do this.

PeteBlackerThe3rd gravatar image PeteBlackerThe3rd  ( 2019-04-09 06:07:11 -0500 )edit