Open fjp opened 3 years ago
I don't actually see how it would be possible for AMCL to publish two transforms with the same timestamp unless it receives two laser scans with the same timestamp - as far as I can see, the only place we publish transforms is inside laserReceived() - and we use the timestamp of the laser message received. To me that indicates either an issue with the Gazebo laser plugin, or some issue in ROS that is causing the same message to be delivered to AMCL twice.
I can confirm that the warning log messages can be reduced when the update rate
of the laser is reduced. Probably there is an issue in the gazebo_ros_gpu_laser
plugin that I am using or maybe there is an issue in gazebo and its sensor
update behaviour.
When using gazebo without the lockstep
parameter, the lidar render thread and the physics thread are not synchronized. When your graphics card can easily keep up and you set your physics update rate low, you are rendering multiple laser scan messages per physics tick which could result in rendering and publishing messages with the same timestamps in the gazebo_ros_gpu_laser
plugin. If you start gazebo
with --lockstep
you tell gazebo that the sensors should respect the update rates that are specified:
--lockstep Lockstep simulation so sensor update rates are respected.
Thanks @reinzor this looks promising. However, I still see the TF warnings being published afer I have added the lockstep
in the mir_empty_world.launch
:
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="world_name" value="$(arg world_name)"/>
<arg name="paused" value="true" />
<arg name="gui" value="$(arg gui)" />
<arg name="lockstep" default="true" />
</include>
I am not sure if I use the lockstep parameter correctly in the launch file.
The empty_world.launch
launch file in the gazebo_ros
package does not consume this parameter:
$ roslaunch gazebo_ros empty_world.launch --ros-args
Required Arguments:
command_arg1: undocumented
command_arg2: undocumented
command_arg3: undocumented
extra_gazebo_args: undocumented
Optional Arguments:
debug (default "false"): undocumented
enable_ros_network (default "true"): undocumented
gui (default "true"): undocumented
gui_required (default "false"): undocumented
headless (default "false"): undocumented
output (default "screen"): undocumented
paused (default "false"): undocumented
physics (default "ode"): undocumented
pub_clock_frequency (default "100"): undocumented
recording (default "false"): undocumented
respawn_gazebo (default "false"): undocumented
server_required (default "false"): undocumented
use_clock_frequency (default "false"): undocumented
use_sim_time (default "true"): undocumented
verbose (default "false"): undocumented
world_name (default "worlds/empty.world"): undocumented
The gzserver
executable in the gazebo_ros
package does:
$ rosrun gazebo_ros gzserver --help
--help -s /opt/ros/noetic/lib/libgazebo_ros_paths_plugin.so -s /opt/ros/noetic/lib/libgazebo_ros_api_plugin.so
gzserver -- Run the Gazebo server.
`gzserver` [options] <world_file>
Gazebo server runs simulation and handles commandline options, starts a Master, runs World update and sensor generation loops.
Options:
-v [ --version ] Output version information.
--verbose Increase the messages written to the terminal.
-h [ --help ] Produce this help message.
-u [ --pause ] Start the server in a paused state.
--lockstep Lockstep simulation so sensor update rates are
respected.
-e [ --physics ] arg Specify a physics engine
(ode|bullet|dart|simbody).
-p [ --play ] arg Play a log file.
-r [ --record ] Record state data.
--record_encoding arg (=zlib) Compression encoding format for log data
(zlib|bz2|txt).
--record_path arg Absolute path in which to store state data
--record_period arg (=-1) Recording period (seconds).
--record_filter arg Recording filter (supports wildcard and regular
expression).
--record_resources Recording with model meshes and materials.
--seed arg Start with a given random number seed.
--iters arg Number of iterations to simulate.
--minimal_comms Reduce the TCP/IP traffic output by gzserver
-s [ --server-plugin ] arg Load a plugin.
-o [ --profile ] arg Physics preset profile name from the options in
the world file.
You could use the extra_gazebo_args
parameter in the launch file here.
I am having the same problem. I tried setting --lockstep
for empty_world but I am still getting the log filled up with the warning.
<include file="$(find gazebo_ros)/launch/empty_world.launch">
[...]
<arg name="extra_gazebo_args" value="--lockstep"/>
</include>
Any other ideas?
This is not an answer to the question, but if you just want to avoid/hide the warnings and analyze the code output on terminal, then you can use the below command at the end of the original command:
2> >(grep -v TF_REPEATED_DATA buffer_core)
Reference: #467
I solved this problem from source . The source code of amcl . To reduce the number of TF_REPEATED_DATA warnings the laser scan publish rate needs to be reduced, because amcl depends on rate of incoming laser messages. I did this in the .cpp node and it totally worked fine for me . If anyone has this issue , you can follow this procedure.
@Daviesss sounds like we want to configure this as parameter instead of setting it hardcoded in the c++-file. Maybe update your code a bit and create a pull-request for AMCL?
@Daviesss sounds like we want to configure this as parameter instead of setting it hardcoded in the c++-file. Maybe update your code a bit and create a pull-request for AMCL?
I don't mind sending a pull request , will do that before the week ends.
Warm regards,
Davies.
@Daviesss sounds like we want to configure this as parameter instead of setting it hardcoded in the c++-file. Maybe update your code a bit and create a pull-request for AMCL?
Sent in a pull request https://github.com/ros-planning/navigation/pull/1260
Warm regards.
Hi, when working with
amcl
for example the mir-robot Gazebo demo (existing map) a lot of the following warnings are logged:I guess this is related to https://github.com/ros/geometry2/issues/467. To reduce the number of
TF_REPEATED_DATA
warnings the laser scan publish rate needs to be reduced, becauseamcl
depends on rate of incoming laser messages.According to the discussion here this seems to be a bug in
amcl
:Can you provide some more hints on how to fix this warning? I assume the problem is in
laserReceived()
?