ros-controls / ros2_controllers

Generic robotic controllers to accompany ros2_control
https://control.ros.org
Apache License 2.0
370 stars 323 forks source link

DiffDriveController is not publishing odom due to the wrong timestamp #532

Open xibeisiber opened 1 year ago

xibeisiber commented 1 year ago

Hi,

I'm trying to simulate the movement my robot in Ignition Gazebo 6, and configured the DiffDriveController following ros2_control_demos.

After the nodes are launched, I found the odom is not always published (Sometimes published, but most time not). I checked the diff_drive_controller.cpp and added some printout, find that previous_publish_timestamp_ = get_node()->get_clock()->now(); sometimes can't get the sim time, but rather get the timestamp in Rviz (1.67733e+09).

In the update() function, the odom will be published only if previous_publish_timestamp_ + publish_period_ < time where "time" is passed in as sim time.

I set use_sim_time to be true in the launch file using

    robot_state_publisher_node = Node(
        package="robot_state_publisher",
        executable="robot_state_publisher",
        output="both",
        parameters=[{"use_sim_time": True}, robot_description],
    )

The console output did have "Setting use_sim_time=True for diff_drive_base_controller":

[ruby $(which ign) gazebo-3] [INFO] [controller_manager]: Setting use_sim_time=True for diff_drive_base_controller to match controller manager (see ros2_control#325 for details)]
[ruby $(which ign) gazebo-3] [INFO] [controller_manager]: Loading controller 'arm_forward_position_controller']
[spawner-6] [INFO] [spawner_diff_drive_base_controller]: Loaded diff_drive_base_controller]
[ruby $(which ign) gazebo-3] [INFO] [controller_manager]: Setting use_sim_time=True for arm_forward_position_controller to match controller manager (see ros2_control#325 for details)]
[ruby $(which ign) gazebo-3] [INFO] [controller_manager]: Configuring controller 'ext_forward_position_controller']
[spawner-8] [INFO] [spawner_arm_forward_position_controller]: Loaded arm_forward_position_controller]
[ruby $(which ign) gazebo-3] [INFO] [ext_forward_position_controller]: configure successful]
[ruby $(which ign) gazebo-3] [INFO] [controller_manager]: Configuring controller 'diff_drive_base_controller']
[ruby $(which ign) gazebo-3] [INFO] [diff_drive_base_controller]: Parameters were updated]
[ruby $(which ign) gazebo-3] previous_publish_timestamp_=1.67733e+09

The strange thing is that this not always happens, sometimes it did get the sim time and publish odom.

Environment: ubuntu 22.04, ros2 humble, Ignition Gazebo 6.11.0

Thanks for any help! Jia

cangozpi commented 1 year ago

I have the same issue and behaviour as you do.

bmagyar commented 1 year ago

Could you please verify if the issue persists on humble?