Hi guys, i was trying to do a localization only using a Lidar (SICK NAV310). So i opted to integration of amcl and rf2o package. But the laser scan wont match with known map when the lidar was moving and the orientation seems inverted.

Here is my launch file:

<launch>
      <node pkg="map_server" type="map_server" name="map_server" args="/home/hilmi/catkin/data/maps/lab_ica_1.yaml" />
      <node pkg="tf" type="static_transform_publisher" name="base_to_laser_broadcaster" args="0 0 0 0 0 0 base_link laser 100" />

      <node pkg="rf2o_laser_odometry" type="rf2o_laser_odometry_node" name="rf2o_laser_odometry" output="screen">
          <param name="laser_scan_topic" value="/sick_nav_3xx/scan" />
          <param name="odom_topic" value="/odom_rf2o" />
          <param name="publish_tf" value="true" />
          <param name="base_frame_id" value="base_link" />
          <param name="odom_frame_id" value="odom" />
          <param name="init_pose_from_topic" value="" />
          <param name="freq" value="8.0"/>
          <param name="verbose" value="true" />
      </node>

      <node pkg="amcl" type="amcl" name="amcl" output="screen">

          <param name="fixed_frame" value="map"/> -->
          <param name="use_map_topic " value="true"/>
          <param name="odom_frame_id" value="odom" />
          <param name="base_frame_id" value="base_link" />
          <param name="frame_id" value="laser" />
          <param name="tf_broadcast" value="true"/>
          <remap from="scan" to="/sick_nav_3xx/scan" />

          <!-- Publish scans from best pose at a max of 10 Hz -->
          <param name="odom_model_type" value="diff-corrected"/>
          <param name="odom_alpha5" value="0.1"/>
          <param name="gui_publish_rate" value="10.0"/>
          <param name="laser_max_beams" value="30"/>
          <param name="min_particles" value="500"/>
          <param name="max_particles" value="5000"/>
          <param name="kld_err" value="0.05"/>
          <param name="kld_z" value="0.99"/>
          <param name="odom_alpha1" value="0.2"/>
          <param name="odom_alpha2" value="0.2"/>
          <!-- translation std dev, m -->
          <param name="odom_alpha3" value="0.8"/>
          <param name="odom_alpha4" value="0.2"/>
          <param name="laser_z_hit" value="0.5"/>
          <param name="laser_z_short" value="0.05"/>
          <param name="laser_z_max" value="0.05"/>
          <param name="laser_z_rand" value="0.5"/>
          <param name="laser_sigma_hit" value="0.2"/>
          <param name="laser_lambda_short" value="0.1"/>
          <param name="laser_model_type" value="likelihood_field"/>
          <!-- <param name="laser_model_type" value="beam"/> -->
          <param name="laser_likelihood_max_dist" value="2.0"/>
          <param name="update_min_d" value="0.2"/>
          <param name="update_min_a" value="0.5"/>
          <param name="odom_frame_id" value="odom"/>
          <param name="resample_interval" value="1"/>
          <param name="transform_tolerance" value="0.1"/>
          <param name="recovery_alpha_slow" value="0.0"/>
          <param name="recovery_alpha_fast" value="0.0"/>

       </node>
    </launch>

(sorry i'm new to github)

And here is my tf_tree and tf_node_graph:

And the thing is when i try to use laser_scan_matcher for the odometry instead of rf2o by following this tutorial and some twekings, it works fine!

Hope anyone can help!
Thanks!

Hi there I'm using rf2o for the navigation stack. I rotated my lidar 90 degrees around z direction, and flipped upside down, and it scans only half range, from -180 to 0 degree.
There I got a problem: tf goes to mirrored direction( x axis)...i.e. when I move robot to right, the tf goes to left..
The tf setup seems to be correct because I run the same tf for hector slam to create a map, and no problem with the pose direction.
I'm new in ROS, could anyone advise how to solve it?

I have a rplidar laser scanner which is rotated by z axis 180 in degree. When I use rf2o to publish the odom calculated from /scan, with correct TF, the pose published has no problem but seems the Twist is not transformed. when moving forward (just started the program), the pose will be moving forward however the twist display a negative linear x velocity. It indicates that the Pose is well-transformed but the Twist not.

Could you please checkout the calculation of lin_speed from acu_trans?

Best regards,
Tian Bo

Hello,

I am trying to publish the odom topic using rf2o_laser_odometry package. I have broadcasted tf messages and scan data before launching the rf2o package. But landed with the error mentioned in below screenshot. Please help me out with this issue.

A quick look at the code let me think that this package depends on mrpt only for using a couple of matrix and two basic math functions.
Wouldn't it make sense to replace mrpt with plain Eigen types such as Eigen::Isometry3d thus removing a - heavy - dependency ? (Heavy regarding its use here...).

Recently, I decided to use rf2o to replace the wheel encoder in my robot_localization EKF node. I'm using Hokuyo as the source of a laser scan topic.

I have tested my system indoor and it works perfectly. But once I moved to the outdoor side, I got this error from rf2o node
([rf2o] ERROR: Eigensolver couldn't find a solution. Pose is not updated)

After searching about that error, I found this commit solves this bug.
artivis@ae01076

After building my workspace, I have nothing new, I got the same error again.

I'm using the map server to provide the map topic for both modes(indoor and outdoor) and the outdoor area with almost flat, I suspect that was the issue.

I hope anyone can help me with this issue, I got stuck by this error.

Thanks!

I'm going to do ros2 with ydlidar x2.
I was going to install slam_toolbox to map it, but I couldn't, so I was advised to install rf2o_laser_odometry. I followed your advice,

ros2 launch ydlidar_ros2_driver ydlidar_launch.py
ros2 run tf2_ros static_transform_publisher 0 0 0 0 0 0 base_link laser_frame
ros2 launch rf2o_laser_odometry rf2o_laser_odometry.launch.py laser_scan_topic:=scan
ros2 launch slam_toolbox online_async_launch.py `base_frame:=base_link`

but there was an error.
[ERROR] [rf2o_laser_odometry_node-1]: exception occurred while executing process:
Traceback (most recent call last):
File "/opt/ros/humble/lib/python3.10/site-packages/launch/actions/execute_local.py", line 546, in __execute_process
transport, self._subprocess_protocol = await async_execute_process(
File "/opt/ros/humble/lib/python3.10/site-packages/osrf_pycommon/process_utils/async_execute_process_asyncio/impl.py", line 139, in async_execute_process
transport, protocol = await _async_execute_process_nopty(
File "/opt/ros/humble/lib/python3.10/site-packages/osrf_pycommon/process_utils/async_execute_process_asyncio/impl.py", line 45, in _async_execute_process_nopty
transport, protocol = await loop.subprocess_exec(
File "/usr/lib/python3.10/asyncio/base_events.py", line 1667, in subprocess_exec
transport = await self._make_subprocess_transport(
File "/usr/lib/python3.10/asyncio/unix_events.py", line 207, in _make_subprocess_transport
transp = _UnixSubprocessTransport(self, protocol, args, shell,
File "/usr/lib/python3.10/asyncio/base_subprocess.py", line 36, in init
self._start(args=args, shell=shell, stdin=stdin, stdout=stdout,
File "/usr/lib/python3.10/asyncio/unix_events.py", line 799, in _start
self._proc = subprocess.Popen(
File "/usr/lib/python3.10/subprocess.py", line 969, in init
self._execute_child(args, executable, preexec_fn, close_fds,
File "/usr/lib/python3.10/subprocess.py", line 1733, in _execute_child
self._posix_spawn(args, executable, env, restore_signals,
File "/usr/lib/python3.10/subprocess.py", line 1678, in _posix_spawn
self.pid = os.posix_spawn(executable, args, env, **kwargs)
OSError: [Errno 8] Exec format error: '/home/car/ros2_ws/install/rf2o_laser_odometry/lib/rf2o_laser_odometry/rf2o_laser_odometry_node'

Help me plz...

when i manually driving the AGV by human , Lidar data inflation along with map by using rf2o .
then i drive by using Ardunio , Lidar data inflation not along with map . I wonder what cause of problem .

system that i use
1.turtlebot3 navigation
2.rf2o odometry

I have set up my lidar node to publish on /scan and the rf2o_laser_odometry to subscribe to /scan, and as you can see with rostopic info /scan, the pub/sub relationship seems correct. However, rf2o_laser_odometry keeps spamming "Waiting for laser_scans..." and doesn't output anything on the odom_rf2o topic.

Any idea what is wrong?

_rostopic info /scan
Type: sensor_msgs/LaserScan

Publishers:

• /rplidarNode
  Subscribers:
• /rf2o_laser_odometry_

I fonud that you take the average of range_last and range_warped and store it in range_inter. But you don't mention it in your paper. So I wonder why you must do that?

Notice that rf2o seems not to be REP117 compliant. If the laser-scan data contains +/- Inf and/or NaNs it fails computing the odom covariance and nothing happens - in the base case scenario.
See a fix on my fork artivis@ae01076.

Originally posted by @artivis in #14 (comment)

Hi thank you for your code.
I ran your code and met an error like this:

[ERROR] [1528530745.560135159]: "base_link" passed to lookupTransform argument target_frame does not exist.

[ INFO] [1528530745.559015472]: [rf2o] Got first Laser Scan .... Configuring node
[ERROR] [1528530745.560135159]: "base_link" passed to lookupTransform argument target_frame does not exist.
[ WARN] [1528530746.569180836]: [rf2o] Waiting for laser_scans....
[ INFO] [1528530746.661969013]: [rf2o] execution time (ms): 91.300003
[ INFO] [1528530746.676619586]: [rf2o] LASERodom = [4069212109502419162988500261007533596520888707133573213859607748348110515496760309981862588138470268977128520817500006621411228751297971167098020859225051821089437351108771130319817666818819669645829339015406038234995868260893555460825585568622722783847935664509926986887330137400246874722674231063085056.000000 4066711308156252521498569006646583609358738761244122237589932837151753773253692050786982401862174574216475381377992770159294349940622886256477694310438370100219056345371907693761619041847029112729922483984368441720851675221915465985159064573964201893388144139083926388282577724119488248465991518744739840.000000 0.000000]
[ERROR] [1528530746.677118076]: "base_link" passed to lookupTransform argument source_frame does not exist.
[ INFO] [1528530747.678349221]: BASEodom = [4069212109502419162988500261007533596520888707133573213859607748348110515496760309981862588138470268977128520817500006621411228751297971167098020859225051821089437351108771130319817666818819669645829339015406038234995868260893555460825585568622722783847935664509926986887330137400246874722674231063085056.000000 4066711308156252521498569006646583609358738761244122237589932837151753773253692050786982401862174574216475381377992770159294349940622886256477694310438370100219056345371907693761619041847029112729922483984368441720851675221915465985159064573964201893388144139083926388282577724119488248465991518744739840.000000 1.570796]

[rf2o] COV_ODO:  6.26805e-06 -8.59658e-06 -3.56644e-06
-8.59658e-06  1.37278e-05  5.87134e-06
-3.56644e-06  5.87134e-06  9.11759e-05

[rf2o] COV_ODO:  2.55766e-06 -6.19651e-08  1.72826e-06
-6.19651e-08   1.3884e-06  1.72536e-06
 1.72826e-06  1.72536e-06  2.55973e-05

[rf2o] COV_ODO:  7.7434e-08 1.39265e-07 2.64753e-07
1.39265e-07  3.1925e-07  5.8949e-07
2.64753e-07  5.8949e-07 4.96849e-06

[rf2o] COV_ODO: 4.31645e-08 -6.6744e-09 2.08869e-08
-6.6744e-09 6.47693e-08 8.08363e-08
2.08869e-08 8.08363e-08   5.405e-07

[rf2o] COV_ODO:  1.78388e-08 -2.09421e-09   5.8128e-09
-2.09421e-09  1.98805e-08 -4.36148e-10
  5.8128e-09 -4.36148e-10  8.87997e-08
[ INFO] [1528530747.942551513]: [rf2o] execution time (ms): 262.619995
[ INFO] [1528530747.944603284]: [rf2o] LASERodom = [4069212109502419162988500261007533596520888707133573213859607748348110515496760309981862588138470268977128520817500006621411228751297971167098020859225051821089437351108771130319817666818819669645829339015406038234995868260893555460825585568622722783847935664509926986887330137400246874722674231063085056.000000 4066711308156252521498569006646583609358738761244122237589932837151753773253692050786982401862174574216475381377992770159294349940622886256477694310438370100219056345371907693761619041847029112729922483984368441720851675221915465985159064573964201893388144139083926388282577724119488248465991518744739840.000000 0.000000]
[ERROR] [1528530747.945146877]: "base_link" passed to lookupTransform argument source_frame does not exist.
[ INFO] [1528530748.946347242]: BASEodom = [4069212109502419162988500261007533596520888707133573213859607748348110515496760309981862588138470268977128520817500006621411228751297971167098020859225051821089437351108771130319817666818819669645829339015406038234995868260893555460825585568622722783847935664509926986887330137400246874722674231063085056.000000 4066711308156252521498569006646583609358738761244122237589932837151753773253692050786982401862174574216475381377992770159294349940622886256477694310438370100219056345371907693761619041847029112729922483984368441720851675221915465985159064573964201893388144139083926388282577724119488248465991518744739840.000000 1.570796]

[rf2o] COV_ODO:  3.99492e-06  -6.1891e-06  1.31348e-06
 -6.1891e-06  9.64553e-06 -2.02555e-06
 1.31348e-06 -2.02555e-06  4.50428e-05

[rf2o] COV_ODO:  5.28336e-06  1.30654e-06 -1.02118e-06

lookupTransform is a function from MRPT.

template <int DIM>
FrameLookUpStatus FrameTransformer<DIM>::lookupTransform(
	const std::string & target_frame,
	const std::string & source_frame,
	typename base_t::lightweight_pose_t & child_wrt_parent,
	const mrpt::system::TTimeStamp query_time,
	const double timeout_secs)
{
	ASSERTMSG_(timeout_secs == .0, "timeout_secs!=0: Blocking calls not supported yet!");
	ASSERTMSG_(query_time == INVALID_TIMESTAMP, "`query_time` different than 'latest' not supported yet!");

	const auto &it_src = m_pose_edges_buffer.find(source_frame);
	if (it_src == m_pose_edges_buffer.end()) {
		return LKUP_UNKNOWN_FRAME;
	}

	const auto &it_dst = it_src->second.find(target_frame);
	if (it_dst == it_src->second.end()) {
		return LKUP_NO_CONNECTIVITY;
	}

	const TF_TreeEdge & te = it_dst->second;
	child_wrt_parent = te.pose;

	return LKUP_GOOD;
}

Is this OK?
I don't know how to solve this.Thank you!

Around line #1041, the publisher node odom begins setting the messages for twist and pose, but the cov_odo calculation is not included. Additionally, the 9 covariance values seems to fluctuate between +- nan when played from a bag file. Is there a reason this isn't added to the publisher? This becomes problematic when attempting to use laser odometry in addition to other forms of odometry in an EKF.

Hi,

I tried your package but I was not able to make it work.
This is the launch file

<launch>
  <node pkg="rf2o_laser_odometry" type="rf2o_laser_odometry_node" name="rf2o_laser_odometry" output="screen">
    <param name="laser_scan_topic" value="/navLaser/scan"/>        # topic where the lidar scans are being published
    <param name="odom_topic" value="/odom_rf2o" />              # topic where tu publish the odometry estimations
    <param name="publish_tf" value="false" />                   # wheter or not to publish the tf::transform (base->odom)
    <param name="base_frame_id" value="/base_link"/>            # frame_id (tf) of the mobile robot base. A tf transform from the laser_frame to the base_frame is mandatory
    <param name="odom_frame_id" value="/odom" />                # frame_id (tf) to publish the odometry estimations
    <param name="freq" value="6.0"/>                            # Execution frequency.
    <param name="verbose" value="true" />                       # verbose
    <param name="init_pose_from_topic" value="" />
  </node>
</launch>

temp.bag.tar.gz

Any suggestion?

does it build fine on melodic ?

For some reason, when I use the base frame id and odom frame id, my robot moves in the exactly opposite direction that it should be, like there's a transformation that is inverted. Any thoughts?

Any issue releasing this code under BSD 3-clause license instead of GPL so it can be used for commercial applications?

In the part of three LIDAR VELOCITY AND 2D RANGE FLOW, how can i identify an observed point P moves with respect to the scanner to P' after an interval of time dt? use ICP or feature matching?

When the robot is standing ideal in sim, it throws the mentioned warnings and as soon as I drive the robot it starts computing the odometry.

Is this behaviour is normal or am I missing something. Also I noticed in odometry message, sometimes why the orientation.w field changes like 0.14 when the robot turns sharp, is this normal adjusting of quaternians? Just wanted to clarify this.

Also, I am having rgb-d intel camera which I simulated in gazebo as well and using point_to_laserscan to get the scan topic and all the scans are having no -+inf.

For this scenario, are there any specific parameter I could include to handle range stuff? @JGMonroy

My odometry message:

I use this package. my computer is ubuntu14.04 ROS indigo, laser lidar is LMS100 SICK
I want use this package, but I meet some problems.
process[rf2o_laser_odometry-2]: started with pid [1068] [ INFO] [1502420195.349735964]: Initializing RF2O node... [ INFO] [1502420195.365088816]: [rf2o] initialization complete...Looping [ WARN] [1502420195.365135745]: [rf2o] Waiting for laser_scans.... [ WARN] [1502420195.385389221]: [rf2o] Waiting for laser_scans.... [ WARN] [1502420195.405355213]: [rf2o] Waiting for laser_scans.... [ WARN] [1502420195.425344404]: [rf2o] Waiting for laser_scans.... [ WARN] [1502420195.445305128]: [rf2o] Waiting for laser_scans.... [ WARN] [1502420195.465332156]: [rf2o] Waiting for laser_scans.... [ WARN] [1502420195.485249243]: [rf2o] Waiting for laser_scans.... [ WARN] [1502420195.505332636]: [rf2o] Waiting for laser_scans.... [ WARN] [1502420195.525351133]: [rf2o] Waiting for laser_scans.... [ WARN] [1502420195.545836174]: [rf2o] Waiting for laser_scans.... [ WARN] [1502420195.565336343]: [rf2o] Waiting for laser_scans.... [ WARN] [1502420195.585384772]: [rf2o] Waiting for laser_scans.... [ WARN] [1502420195.605366646]: [rf2o] Waiting for laser_scans.... [ WARN] [1502420195.625333745]: [rf2o] Waiting for laser_scans.... [ WARN] [1502420195.645346097]: [rf2o] Waiting for laser_scans.... [ WARN] [1502420195.665551894]: [rf2o] Waiting for laser_scans.... [ WARN] [1502420195.685242988]: [rf2o] Waiting for laser_scans.... [ WARN] [1502420195.705334549]: [rf2o] Waiting for laser_scans..
I find in rf2o_laser_odometry.launch but I do not know the mean
<param name="init_pose_from_topic" value="/base_pose_ground_truth" /> # (Odom topic) Leave empty to start at point (0,0)
I see the source code CLaserOdometry2D.cpp . could you please tell what happen? thx
if( myLaserOdom.is_initialized() && myLaserOdom.scan_available() ) { //Process odometry estimation myLaserOdom.odometryCalculation(); } else { ROS_WARN("[rf2o] Waiting for laser_scans....") ; }

when pure rotation,the result degrade?

Hi,
I would like to use your scan matcher within another project but it seems that the whole implementation takes places in the node itself.
Is there any plan to separate the actual scan matching from the node therefore providing a usable API ??

Cheers.

I have tested rf2o in hallways with width approx. 2m * 12m dimensions and it won't work. Though it works fine in feature-rich rooms, as soon as the robot enters the hallway it hardly recognizes the motion of robot, The wall at the end of the hallway will not help with localizing the robot. I suggest that its fitting process consider the distance of scanned points to the robot and weight it accordingly, since further points have less density and should take same weights as the points near-by. Also the global covariance matrix is 0, which makes it hard to integrate with other filtering algorithms such as EKF.

Hello author!

I like your project. thank you so much to share your program.

I have a small question.

i found a covariance matrix in Claserodometry2D.cpp line 719

cov_odo = (1.f/float(num_valid_range-3))*AtA.inverse()*res.squaredNorm();

this is covariance value, right?

now I saw that matrix is 3x3.

In the matrix, I understand that value is in below:

t=theta

x-x x-y x-t
y-x y-y y-t
t-x t-y t-t

this is correct?

Thank you so much

best regards

Hello. First of all, thanks for this package.

I'm following aws-deepracer tutorial.
They provided the guide with ros2 package included rf2o_laser_odometry package to handle the deepracer robot.
But, someone who want to know this package couldn't find ros2 branch on main page of this repository, because there is no tag and branch for ros2. They can only find the commit through the script.

Could you crate tag or branch for ros2 user?
This(#29) is suitable commit for this issue.