search

ros-navigation / navigation2_tutorials

Tutorial code referenced in https://docs.nav2.org/
198 stars 130 forks source link

No data on /odometry/filtered topic #30

Closed maarufvazifdar closed 2 years ago

maarufvazifdar commented 3 years ago

I was following the turorial to try ekf on ros2 foxy. On launching display.launch.py bot spawned in gazebo and rviz, and I can see the ekf topics but on echoing /odometry/filtered there is no message being published. I checked and there is data is getting published on /demo/odom and /demo/imu.

I am running Ubuntu 20.04 and ros-foxy.

Here are the published topics: 

/accel/filtered
/clock
/demo/cmd_vel
/demo/imu
/demo/odom
/diagnostics
/joint_states
/odometry/filtered
/parameter_events
/robot_description
/rosout
/set_pose
/tf
/tf_static

Ekf node info: 

/ekf_filter_node
  Subscribers:
    /clock: rosgraph_msgs/msg/Clock
    /demo/imu: sensor_msgs/msg/Imu
    /demo/odom: nav_msgs/msg/Odometry
    /parameter_events: rcl_interfaces/msg/ParameterEvent
    /set_pose: geometry_msgs/msg/PoseWithCovarianceStamped
  Publishers:
    /accel/filtered: geometry_msgs/msg/AccelWithCovarianceStamped
    /diagnostics: diagnostic_msgs/msg/DiagnosticArray
    /odometry/filtered: nav_msgs/msg/Odometry
    /parameter_events: rcl_interfaces/msg/ParameterEvent
    /rosout: rcl_interfaces/msg/Log
    /tf: tf2_msgs/msg/TFMessage
  Service Servers:
    /ekf_filter_node/describe_parameters: rcl_interfaces/srv/DescribeParameters
    /ekf_filter_node/get_parameter_types: rcl_interfaces/srv/GetParameterTypes
    /ekf_filter_node/get_parameters: rcl_interfaces/srv/GetParameters
    /ekf_filter_node/list_parameters: rcl_interfaces/srv/ListParameters
    /ekf_filter_node/set_parameters: rcl_interfaces/srv/SetParameters
    /ekf_filter_node/set_parameters_atomically: rcl_interfaces/srv/SetParametersAtomically
    /enable: std_srvs/srv/Empty
    /set_pose: robot_localization/srv/SetPose
    /toggle: robot_localization/srv/ToggleFilterProcessing
  Service Clients:

  Action Servers:

  Action Clients:

Launch log:

1620618863.7946322 [INFO] [launch]: All log files can be found below /home/maaruf/.ros/log/2021-05-10-09-24-23-791827-maaruf-Strix-15-GL503GE-9948
1620618863.7947869 [INFO] [launch]: Default logging verbosity is set to INFO
1620618863.9245000 [INFO] [gazebo-1]: process started with pid [9951]
1620618863.9249437 [INFO] [robot_state_publisher-2]: process started with pid [9953]
1620618863.9251087 [INFO] [spawn_entity.py-3]: process started with pid [9955]
1620618863.9252398 [INFO] [ekf_node-4]: process started with pid [9957]
1620618863.9253635 [INFO] [rviz2-5]: process started with pid [9959]
1620618863.9446864 [robot_state_publisher-2] Parsing robot urdf xml string.
1620618863.9452262 [robot_state_publisher-2] The root link base_link has an inertia specified in the URDF, but KDL does not support a root link with an inertia.  As a workaround, you can add an extra dummy link to your URDF.
1620618863.9453862 [robot_state_publisher-2] Link base_footprint had 0 children
1620618863.9454966 [robot_state_publisher-2] Link front_caster had 0 children
1620618863.9455602 [robot_state_publisher-2] Link drivewhl_l_link had 0 children
1620618863.9456286 [robot_state_publisher-2] Link drivewhl_r_link had 0 children
1620618863.9457018 [robot_state_publisher-2] Link imu_link had 0 children
1620618863.9457703 [robot_state_publisher-2] [INFO] [1620618863.944830383] [robot_state_publisher]: got segment base_footprint
1620618863.9458444 [robot_state_publisher-2] [INFO] [1620618863.944961115] [robot_state_publisher]: got segment base_link
1620618863.9459641 [robot_state_publisher-2] [INFO] [1620618863.944977239] [robot_state_publisher]: got segment drivewhl_l_link
1620618863.9460473 [robot_state_publisher-2] [INFO] [1620618863.944983669] [robot_state_publisher]: got segment drivewhl_r_link
1620618863.9465437 [robot_state_publisher-2] [INFO] [1620618863.944988418] [robot_state_publisher]: got segment front_caster
1620618863.9466844 [robot_state_publisher-2] [INFO] [1620618863.944994314] [robot_state_publisher]: got segment imu_link
1620618865.6764433 [INFO] [spawn_entity.py-3]: process has finished cleanly [pid 9955]

Please let me know if more information is required about the issue. @SteveMacenski

WUyinwei-hah commented 2 years ago

I have the same problem. Have you solved it?

SteveMacenski commented 2 years ago

@arubenecia @jd-deleon can you take a look?

By the way, these tutorials were written for Galactic and newer. I'm not sure if that has anything to do with the issues you're seeing, but just giving you that heads up.

arubenecia commented 2 years ago

We did not notice this when making the tutorial but upon checking, we also encountered the same problem on Galactic.

We have observed that data is being published on /odometry/filtered and /accel/filtered after setting ekf_node's use_sim_time to false (https://github.com/ros-planning/navigation2_tutorials/blob/master/sam_bot_description/launch/display.launch.py#L41).

But doing this causes outdated odom->base_link transform by the ekf_node (TF_OLD_DATA error shows for base_link). Since odom->base_link transform is already being published by Gazebo's differential drive plugin, we set ekf_node's publish_tf to false (https://github.com/ros-planning/navigation2_tutorials/blob/master/sam_bot_description/config/ekf.yaml#L19) to solve the TF_OLD_DATA error.

After setting use_sim_time and publish_tf of ekf_node to false, the data are are now being published on /odometry/filtered and /accel/filtered

We are not yet sure what is the cause of the problem but we will look into this more next week.

WUyinwei-hah commented 2 years ago

We did not notice this when making the tutorial but upon checking, we also encountered the same problem on Galactic.

We have observed that data is being published on /odometry/filtered and /accel/filtered after setting ekf_node's use_sim_time to false (https://github.com/ros-planning/navigation2_tutorials/blob/master/sam_bot_description/launch/display.launch.py#L41).

But doing this causes outdated odom->base_link transform by the ekf_node (TF_OLD_DATA error shows for base_link). Since odom->base_link transform is already being published by Gazebo's differential drive plugin, we set ekf_node's publish_tf to false (https://github.com/ros-planning/navigation2_tutorials/blob/master/sam_bot_description/config/ekf.yaml#L19) to solve the TF_OLD_DATA error.

After setting use_sim_time and publish_tf of ekf_node to false, the data are are now being published on /odometry/filtered and /accel/filtered

We are not yet sure what is the cause of the problem but we will look into this more next week.

ok, thanks. I do some log and find that in ros_filter.cpp the code can not run into the function periodicUpdate, take it a step further the GenericTimer seems not work properly. Is that caused by the parameter use_sim_time?

ayrton04 commented 2 years ago

Is that caused by the parameter use_sim_time?

In Foxy, if use_sim_time is set to true, but nothing is getting published on /clock, then the node will hang indefinitely. I am not familiar with this tutorial, so this might be something else.

SteveMacenski commented 2 years ago

That makes sense

arubenecia commented 2 years ago

Is that caused by the parameter use_sim_time?

In Foxy, if use_sim_time is set to true, but nothing is getting published on /clock, then the node will hang indefinitely. I am not familiar with this tutorial, so this might be something else.

Thank you. It turns out that Gazebo is not publishing to /clock.

@maarufvazifdar @WUyinwei-hah The problem was fixed by loading the gazebo_ros_init plugin which publishes the Gazebo simulation time to /clock.

Please try loading the gazebo_ros_init by changing line https://github.com/ros-planning/navigation2_tutorials/blob/master/sam_bot_description/launch/display.launch.py#L53 to launch.actions.ExecuteProcess(cmd=['gazebo', '-s', 'libgazebo_ros_init.so', '-s', 'libgazebo_ros_factory.so', world_path], output='screen'),

The world_path was added in the next tutorial, 'Setting Up Sensors', so you can remove world_path in the line if you have not applied the changes in that tutorial to display.launch.py yet.

Also no need to change use_sim_time and publish_tfof ekf_node to false.

We'll file PR for the changes in the tutorial and source code.

@SteveMacenski We are also thinking of setting the differential drive plugin's <publish_odom_tf> to false since we already set publish_tfof ekf_node to true. We will include this in the PR if you agree.

WUyinwei-hah commented 2 years ago

Is that caused by the parameter use_sim_time?

In Foxy, if use_sim_time is set to true, but nothing is getting published on /clock, then the node will hang indefinitely. I am not familiar with this tutorial, so this might be something else.

Thank you. It turns out that Gazebo is not publishing to /clock.

@maarufvazifdar @WUyinwei-hah The problem was fixed by loading the gazebo_ros_init plugin which publishes the Gazebo simulation time to /clock.

Please try loading the gazebo_ros_init by changing line https://github.com/ros-planning/navigation2_tutorials/blob/master/sam_bot_description/launch/display.launch.py#L53 to launch.actions.ExecuteProcess(cmd=['gazebo', '-s', 'libgazebo_ros_init.so', '-s', 'libgazebo_ros_factory.so', world_path], output='screen'),

The world_path was added in the next tutorial, 'Setting Up Sensors', so you can remove world_path in the line if you have not applied the changes in that tutorial to display.launch.py yet.

Also no need to change use_sim_time and publish_tfof ekf_node to false.

We'll file PR for the changes in the tutorial and source code.

@SteveMacenski We are also thinking of setting the differential drive plugin's <publish_odom_tf> to false since we already set publish_tfof ekf_node to true. We will include this in the PR if you agree.

Thanks for your work!

SteveMacenski commented 2 years ago

Please do!