unthrottle ros controller update rates for the multicamera to prevent mismatch between gazebo image rates and ros image publish rates.

#125 Merged at dbcfa5e
Repository
Branch
issue_123
Repository
Branch
drcsim_2.2
Author
  1. John Hsu
Reviewers
Description

added ros topic /multisense_sl/fps for changing camera update rate.

currently, the resolution is locked to 1024 X 544 @ 1 - 60Hz.

change foot friction to 0.9 until further validation results are available.

per question removed filter on feet contact force torque outputs.

Comments (12)

  1. Thomas Koletschka

    Hmm ... this solves it a bit too well for me. I'm getting framerates of >50 now within gazebo and ros even though <sensor name="stereo_camera" type="multicamera"> still has an update_rate of 15 and only <plugin filename="libgazebo_ros_multicamera.so" name="stereo_camera_controller"> has an updateRate of 0.

    Same effect with the lidar. When i set updateRate within <plugin filename="libgazebo_ros_laser.so" name="gazebo_ros_head_hokuyo_controller"> to 80 then both gazebo and ros rate jump to ~75 even though <sensor name="head_hokuyo_sensor" type="ray"> still has its update_rate set to 40.

    This is with the threaded_sensors branch.

    1. John Hsu author

      Indeed this is a bug where setting plugin frame rates gets propagated upstream to sensor update rates. Pull request updated to remove to call to update parent sensor.

      1. Thomas Koletschka

        Works, although there's still some slight offset of ~2 fps between Gazebo and ROS when running at higher framerates than the default 15fps, but I guess that's due to the ROS plugin currently being updated independent of the Gazebo sensor.

        Should the same fix (not setting the parent's update rate) be done for the laser sensors (and probably others)?

        1. John Hsu author

          yes, went through and made sure the other plugins do not set parent sensor update rates automatically.

  2. Thomas Koletschka

    I also noticed that you don't seem to synchronize camera info messages with their corresponding images. With the fix in this pr camera info gets published at close to 1kHz (i.e. simulator rate) while the corresponding images get published at ~55Hz on my machine.

    1. John Hsu author

      will find out if camera_info and image_raw are synchronized on the actual hardware. But we should throttle camera_info at the same rate that image_raw is publishing. Given camera_info contain mostly static data, as we are not expecting users to dynamically change camera settings from frame to frame, we'll likely punt on synchronization between the two unless this is a necessary feature.

      1. Thomas Koletschka

        camera_info and image_raw is synchronized in drivers for real cameras as it's simply published alongside the image in the frame grabber callback using image_transport::CameraPublisherwhich takes both the image and the camera info as arguments for publishing (e.g. image_pub_.publish(image, ci); in camera1394).

        Publishing it every simulation step instead of just when an image gets published isn't really a problem as there will always be a camera_info with a timestamp that corresponds to the image (image processing nodes usually require synchronized camera_info - image pairs) but only publishing it with the image would save you from spamming all this data at 1kHz (as you correctly pointed out it does not change often).

  3. Thomas Koletschka
    Dbg should not be in here, somnething may be wrong on startup.  Sensor::SetActive has been called, so rendering::Camera is actively publishing images, but MultiCameraSensor::IsActive() returns false
    

    is this intended to show up when unsubscribing from the sensor?