Cannot get odom->base_link from laser_scan_matcher

asked 2018-04-05 17:25:47 -0600

trixr4kdz gravatar image

updated 2018-04-09 04:42:06 -0600

Hi,

I have a scanse spinning lidar that I'm trying to use to generate a map using gmapping. Right now I have a base_link->laser_frame tf that I am publishing using tf2_ros. Since gmapping requires a odom->base_link tf, I am now trying to use laser_scan_matcher to generate that transform.

image description

However, I'm stuck with trying to generate the odom->base_link. Here is the launch file I'm using to run laser_scan_matcher:

<?xml version="1.0"?>

<launch>
  <node pkg="mavros" type="mavros_node" name="mavros" clear_params="true">
    <param name="fcu_url" value="tcp://127.0.0.1:5760" />
    <param name="system_id" value="1" />
  </node>

  <node 
    pkg="laser_scan_matcher"
    type="laser_scan_matcher_node"
    name="laser_scan_matcher_node"
    output="screen"
  >
    <param name="base_frame" value="base_link" />
    <param name="fixed_frame" value="odom" />
    <param name="use_imu" value="true" />
    <param name="use_odom" value="false" />
    <param name="use_vel" value="false" />
    <param name="max_iterations" value="10" />
    <param name="publish_tf" value="true" />
    <param name="publish_pose" value="true" />

    <remap from="imu/data" to="/mavros/imu/data" />
    <remap from="scan" to="/camera/scan" />
  </node>
</launch>

And here is my launch file for running my scanse lidar:

<launch>
  <param name="robot_description" command="$(find xacro)/xacro --inorder '$(find realsense_ros_camera)/urdf/test_r200_camera.urdf.xacro'" />
  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />

  <node pkg="tf2_ros" type="static_transform_publisher" name="base_link_2_camera_link" args="0 0 0 0 0 0 base_link camera_link"/>

  <node pkg="tf2_ros" type="static_transform_publisher" name="base_link_to_laser" args="0.13 0 0 0 0 0 base_link laser"/>

  <!-- run sweep_node node -->
  <node 
    name="sweep_node"          
    pkg="sweep_ros"  
    type="sweep_node" 
    output="screen">
      <param name="serial_port"         type="string" value="/dev/ttyUSB0"/>
      <param name="serial_baudrate"     type="int"    value="115200"/>
      <param name="frame_id"            type="string" value="laser"/>
      <param name="rotation_speed" value="5" />
  </node>

  <!-- run pointcloud_to_laserscan node -->
  <node pkg="pointcloud_to_laserscan" type="pointcloud_to_laserscan_node" name="pointcloud_to_laserscan">

    <remap from="cloud_in" to="pc2"/>
    <remap from="scan"  to="/camera/scan"/>

    <rosparam>
        target_frame: "laser" 
        transform_tolerance: 0.001
        min_height: -0.5
        max_height: 0.5

        angle_min: -2.14 
        angle_max: 2.14 
        angle_increment: 0.005 
        scan_time: 0.1
        range_min: 0.1
        range_max: 40.0
        use_inf: true

        concurrency_level: 1
    <rosparam />
  </node>
</launch>

Additionally, I'm getting the following warning over and over when running laser_scan_matcher:

:err: Valid: 31/857 invalid: 826.
[ WARN] [1523266642.454564818]: Error in scan matching

I am using Ubuntu 16.04 with ROS Kinetic with the scanse sweep-ros wrapper.

Also, here is an output of /camera/scan:

header: 
  seq: 1262
  stamp: 
    secs: 1523266739
    nsecs: 995312062
  frame_id: "laser"
angle_min: -2.1400001049
angle_max: 2.1400001049
angle_increment: 0.00499999988824
time_increment: 0.0
scan_time: 0.10000000149
range_min: 0.10000000149
range_max: 40.0
ranges: [1.5499999523162842, inf, inf, inf, inf, inf, inf, inf, inf, 1.5499999523162842, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, 1.5700000524520874, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, 1.4800000190734863, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf ...
(more)
edit retag flag offensive close merge delete

Comments

2

please don't post screenshots of text, as this is not searchable. Just copy the text Output of the console, and paste it (by editing your question) and Format using the preformatted text button (101010) as code.

mgruhler gravatar imagemgruhler ( 2018-04-06 00:44:33 -0600 )edit
1

please also provide more info about the laserscanner you use. Best also edit your question and include the configuration of the laser scanner, the Launch file with which you start it and maybe the Output of a rostopic echo /camera/scan (if this is the topic).

mgruhler gravatar imagemgruhler ( 2018-04-06 00:46:13 -0600 )edit

Hi @mig, apologies for that. I've edited my post to reflect your comment. Thanks!

trixr4kdz gravatar imagetrixr4kdz ( 2018-04-09 04:37:58 -0600 )edit

The scan topic doesn't look good. There are IMO way to many inf readings. Is this something that makes sense for your Environment? If not, you should tweak the pointcloud_to_laserscan parameters. Yours differ from the ones in the sweep-ros repo quite a lot.

mgruhler gravatar imagemgruhler ( 2018-04-09 06:46:55 -0600 )edit

If the Output is ok, scan matching will definitely be hard with only that Little valid measurements...

mgruhler gravatar imagemgruhler ( 2018-04-09 06:47:20 -0600 )edit

We changed the parameters for sweep-ros because we've observed that it works better for our particular use case. However, I've also tested the laser_scan_matcher with the same parameters as the example and the "Error in scan matching" warning remains.

trixr4kdz gravatar imagetrixr4kdz ( 2018-04-11 01:44:38 -0600 )edit

So unless we can minimize the amount of inf readings/remove them completely, we're always going to get that warning? And would you say that the "Error in scan matching" is the cause as to why we are not able to get the odom->base_link transform?

trixr4kdz gravatar imagetrixr4kdz ( 2018-04-11 01:46:17 -0600 )edit

I guess so. The scan_matcher provides odom->base_link on the basis of the matched scans. If it cannot do that, I guess it will not publish an incorrect transform (but I could be wrong). It could also be a problem of frequency. I don't know how many updates are required for the scan matching to ...

mgruhler gravatar imagemgruhler ( 2018-04-12 01:10:47 -0600 )edit