Transform tree damaged after upgrading ROS

edit

Hello ROS community,

I am trying to upgrade from Ubuntu 16.04 + ROS kinetic + Gazebo 7 to Ubuntu 18.04 + Ros Melodic + Gazebo 9.

I have a ROSLaunch file that runs gazebo with an environment and a node that publishes the ground truth position of the urdf robot. However, when running the node after the upgrade I get the error:

[ERROR] [1549876249.264445531, 7.551000000]: "odom" passed to lookupTransform argument target_frame does not exist. 
[ERROR] [1549876249.795771873, 8.077000000]: Lookup would require extrapolation at time 7.555000000, but only time 7.051000000 is in the buffer, when looking up transform from frame [camera_link] to frame [odom]

The transformation tree looks like this:

while in the previous version looked like this:

The launch file is:

<launch>

  <!-- Launch the empty world -->
  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="world_name" value="worlds/empty.world"/> 
    <arg name="paused" value="false"/>
    <arg name="use_sim_time" value="true"/>
   <arg name="gui" value="true"/>
    <arg name="recording" value="false"/>
    <arg name="debug" value="false"/>
  </include>

  <!-- Spawn the apartment into Gazebo -->
  <arg name="environment" default="test_apartment_2" />

  <node name="spawn_sdf" pkg="gazebo_ros" type="spawn_model" 
        args="-sdf -file $(find lucrezio_simulation_environments)/models/$(arg environment)/model.sdf -model $(arg environment)" />


    <!-- Upload object locations and spawn them into Gazebo-->
    <include file="$(find lucrezio_simulation_environments)/launch/upload_object_locations.launch">
    <arg name="environment" value="$(arg environment)" />
    </include>

  <node name="spawn_objects" pkg="lucrezio_simulation_environments" type="spawn_object.py" args="all"/>

  <!-- push robot_description to factory and spawn robot in gazebo -->
  <arg name="model" default="$(find lucrezio_simulation_environments)/urdf/lucrezio_with_logical.urdf.xacro"/>
  <param name="robot_description" command="$(find xacro)/xacro $(arg model)" />
  <node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model"
        args="-z 1.0 -unpause -urdf -model robot -param robot_description" respawn="false" output="screen" />

  <node pkg="tf" type="static_transform_publisher" name="camera_link_broadcaster" args="0 0 0 -0.5 0.5 -0.5 0.5 /camera_link /camera_depth_optical_frame 20" />
  <node pkg="tf" type="static_transform_publisher" name="base_footprint_broadcaster" args="0 0 0.1 0.0 0.0 0.0 1.0 /base_footprint /base_link 20" />

  <!-- start state publishers -->
  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
  <node pkg="robot_state_publisher" type="robot_state_publisher"  name="robot_state_publisher"/>
<!--    <param name="publish_frequency" type="double" value="100.0" />
  </node>
-->

  <!-- Run the controller manager -->
  <rosparam command="load" file="$(find lucrezio_simulation_environments)/config/diffdrive.yaml" ns="lucrezio" />
  <node name="lucrezio_controller_spawner" pkg="controller_manager" type="spawner" args="lucrezio --shutdown-timeout 3"/>

</launch>

and the node:

#include <iostream>
#include <ros/ros.h>
#include <tf/tf.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_listener.h>
#include <tf/transform_broadcaster.h>
#include <gazebo_msgs/LinkStates.h>
#include <Eigen/Geometry>
#include <sensor_msgs/LaserScan.h>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <lucrezio_simulation_environments/LogicalImage.h>

class PoseBroadcaster{
public:
  PoseBroadcaster(ros::NodeHandle nh_):
    _nh(nh_),
    _logical_image_sub(_nh,"/gazebo/logical_camera_image",1),
    _scan_sub(_nh,"/scan",1),
    _synchronizer(FilterSyncPolicy(1000),_logical_image_sub,_scan_sub){
    _synchronizer.registerCallback(boost::bind(&PoseBroadcaster::filterCallback, this, _1, _2));
  }

  void filterCallback(const lucrezio_simulation_environments::LogicalImage::ConstPtr &logical_image_msg,
                      const sensor_msgs::LaserScan::ConstPtr &laser_msg){
    ros::Time laser_stamp = laser_msg->header.stamp;
    tf::StampedTransform camera_odom_tf;
    try{
      _listener.waitForTransform("/odom",
                                 "/camera_link",
                                 laser_stamp,
                                 ros::Duration(0.5));
      _listener.lookupTransform("/odom",
                                "/camera_link",
                                laser_stamp,
                                camera_odom_tf ...