ros-drivers / joystick_drivers

ROS drivers for joysticks
http://wiki.ros.org/joystick_drivers
206 stars 292 forks source link

coalesce_interval_ms parameter has no effect #284

Open CaptKrasno opened 6 months ago

CaptKrasno commented 6 months ago

I am trying to limit the data usage of my system by reducing the maximum rate of joy message transmission. To accomplish this in the past (in ros1) I would lower the auto repeat rate and the coalescence_interval. However, in the ros2 version the coalescence_interval_ms seems to have no effect. Here are the parameters from my node:

/odysseus_common/control/console_joystick:
  ros__parameters:
    autorepeat_rate: 2.0
    coalesce_interval_ms: 100
    deadzone: 0.05
    device_id: 0
    device_name: ''
    qos_overrides:
      /parameter_events:
        publisher:
          depth: 1000
          durability: volatile
          history: keep_last
          reliability: reliable
    sticky_buttons: false
    use_sim_time: false

when I move the sticks rapidly I am still seeing data rates well over 200hz

Jannled commented 4 months ago

Well I stumbled upon the same issue. If you need a quick and dirty fix, clone the ros2 branch and replace the lines (455 - 474) in joy/src/joy.cpp

if (!should_publish) {
  // So far, nothing has indicated that we should publish.  However we need to
  // do additional checking since there are several possible reasons:
  // 1.  SDL_WaitEventTimeout failed
  // 2.  SDL_WaitEventTimeout timed out
  // 3.  SDL_WaitEventTimeout succeeded, but the event that happened didn't cause
  //     a publish to happen.
  //
  // If we are autorepeating and enough time has passed, set should_publish.
  rclcpp::Time now = this->now();
  rclcpp::Duration diff_since_last_publish = now - last_publish;
  if ((autorepeat_rate_ > 0.0 &&
    RCL_NS_TO_MS(diff_since_last_publish.nanoseconds()) >= autorepeat_interval_ms_) ||
    publish_soon_)
  {
    last_publish = now;
    should_publish = true;
    publish_soon_ = false;
  }
}

with the following:

if(should_publish) {
  const rclcpp::Time now = this->now();
  const rclcpp::Duration diff_since_last_publish = now - last_publish;

  if(RCL_NS_TO_MS(diff_since_last_publish.nanoseconds()) < coalesce_interval_ms_)
    should_publish = false;
  else
    last_publish = now;
}

Then just recompile as usual (source /opt/ros/iron/setup.bash, colcon build, source install/local_setup.bash)

After my bachelor thesis I might have the time to look at what the actual issue is and submit a pull request. But maybe one of the maintainers can figure this out faster.

Jannled commented 4 months ago

And a minor inconvenience: The params like coalesce_interval_ms can't be changed during runtime with the ros2 param set command. But this is not a problem if you use ros2 launch sample.xml:

<launch>
    <node pkg="joy" exec="joy_node">
        <param name="coalesce_interval_ms" value="500"/>
        <param name="autorepeat_rate" value="0.0"/><!-- Unit in ns -->
    </node>
</launch>
Peppe-Grasso commented 4 weeks ago

it seems like setting publish_soon_ to false is essentially bypassing the coalesce check in handleJoyAxis() and is therefore causing the coalesce_interval_ms parameter to not do anything. The fix above also seems to prevent autorepeat form working. For my use case, removing the publish_soon_ stuff has resulted in what i believe to be the expected behaviour,

i.e. replace lines (455 - 474)

if (!should_publish) {
  // So far, nothing has indicated that we should publish.  However we need to
  // do additional checking since there are several possible reasons:
  // 1.  SDL_WaitEventTimeout failed
  // 2.  SDL_WaitEventTimeout timed out
  // 3.  SDL_WaitEventTimeout succeeded, but the event that happened didn't cause
  //     a publish to happen.
  //
  // If we are autorepeating and enough time has passed, set should_publish.
  rclcpp::Time now = this->now();
  rclcpp::Duration diff_since_last_publish = now - last_publish;
  if ((autorepeat_rate_ > 0.0 &&
    RCL_NS_TO_MS(diff_since_last_publish.nanoseconds()) >= autorepeat_interval_ms_) ||
    publish_soon_)
  {
    last_publish = now;
    should_publish = true;
    publish_soon_ = false;
  }
}

with

if (!should_publish) {
  // So far, nothing has indicated that we should publish.  However we need to
  // do additional checking since there are several possible reasons:
  // 1.  SDL_WaitEventTimeout failed
  // 2.  SDL_WaitEventTimeout timed out
  // 3.  SDL_WaitEventTimeout succeeded, but the event that happened didn't cause
  //     a publish to happen.
  //
  // If we are autorepeating and enough time has passed, set should_publish.
  rclcpp::Time now = this->now();
  rclcpp::Duration diff_since_last_publish = now - last_publish;
  if (autorepeat_rate_ > 0.0 && RCL_NS_TO_MS(diff_since_last_publish.nanoseconds()) >= autorepeat_interval_ms_) 
  {
    last_publish = now;
    should_publish = true;
  }
}

I can't think of a reason why publish_soon_ should be checked and set to true in lines (455-474), although I haven't really looked into it properly